1 package com.irurueta.navigation.inertial.calibration;
2
3 import com.irurueta.algebra.Matrix;
4 import com.irurueta.algebra.WrongSizeException;
5 import com.irurueta.navigation.LockedException;
6 import com.irurueta.navigation.NotReadyException;
7 import com.irurueta.navigation.frames.CoordinateTransformation;
8 import com.irurueta.navigation.frames.ECEFFrame;
9 import com.irurueta.navigation.frames.FrameType;
10 import com.irurueta.navigation.frames.InvalidSourceAndDestinationFrameTypeException;
11 import com.irurueta.navigation.frames.NEDFrame;
12 import com.irurueta.navigation.frames.converters.NEDtoECEFFrameConverter;
13 import com.irurueta.navigation.frames.converters.NEDtoECEFPositionVelocityConverter;
14 import com.irurueta.navigation.geodesic.Constants;
15 import com.irurueta.navigation.inertial.BodyKinematics;
16 import com.irurueta.navigation.inertial.ECEFGravity;
17 import com.irurueta.navigation.inertial.ECEFPosition;
18 import com.irurueta.navigation.inertial.ECEFVelocity;
19 import com.irurueta.navigation.inertial.NEDPosition;
20 import com.irurueta.navigation.inertial.NEDVelocity;
21 import com.irurueta.navigation.inertial.estimators.ECEFGravityEstimator;
22 import com.irurueta.navigation.inertial.estimators.ECEFKinematicsEstimator;
23 import com.irurueta.statistics.UniformRandomizer;
24 import com.irurueta.units.Acceleration;
25 import com.irurueta.units.AccelerationUnit;
26 import org.junit.Ignore;
27 import org.junit.Test;
28
29 import java.util.ArrayList;
30 import java.util.Collection;
31 import java.util.Collections;
32 import java.util.List;
33 import java.util.Random;
34
35 import static org.junit.Assert.*;
36
37 public class KnownPositionAccelerometerCalibratorTest implements
38 KnownPositionAccelerometerCalibratorListener {
39
40 private static final double TIME_INTERVAL_SECONDS = 0.02;
41
42 private static final double MICRO_G_TO_METERS_PER_SECOND_SQUARED = 9.80665E-6;
43 private static final double DEG_TO_RAD = 0.01745329252;
44
45 private static final double MIN_ANGLE_DEGREES = -180.0;
46 private static final double MAX_ANGLE_DEGREES = 180.0;
47
48 private static final double MIN_LATITUDE_DEGREES = -90.0;
49 private static final double MAX_LATITUDE_DEGREEs = 90.0;
50 private static final double MIN_LONGITUDE_DEGREES = -180.0;
51 private static final double MAX_LONGITUDE_DEGREES = 180.0;
52 private static final double MIN_HEIGHT = -50.0;
53 private static final double MAX_HEIGHT = 50.0;
54
55 private static final int LARGE_MEASUREMENT_NUMBER = 100000;
56
57 private static final double ABSOLUTE_ERROR = 1e-8;
58 private static final double LARGE_ABSOLUTE_ERROR = 5e-5;
59
60 private static final int TIMES = 100;
61
62 private int mCalibrateStart;
63 private int mCalibrateEnd;
64
65 @Test
66 public void testConstructor1() throws WrongSizeException {
67 final KnownPositionAccelerometerCalibrator calibrator =
68 new KnownPositionAccelerometerCalibrator();
69
70
71 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
72 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
73 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
74 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
75 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
76 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
77 final Acceleration bx2 = new Acceleration(0.0,
78 AccelerationUnit.FEET_PER_SQUARED_SECOND);
79 calibrator.getInitialBiasXAsAcceleration(bx2);
80 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
81 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
82 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
83 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
84 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
85 final Acceleration by2 = new Acceleration(0.0,
86 AccelerationUnit.FEET_PER_SQUARED_SECOND);
87 calibrator.getInitialBiasYAsAcceleration(by2);
88 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
89 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
90 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
91 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
92 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
93 final Acceleration bz2 = new Acceleration(0.0,
94 AccelerationUnit.FEET_PER_SQUARED_SECOND);
95 calibrator.getInitialBiasZAsAcceleration(bz2);
96 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
97 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
98 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
99 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
100 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
101 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
102 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
103 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
104 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
105 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
106 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
107 final double[] bias1 = calibrator.getInitialBias();
108 assertArrayEquals(bias1, new double[3], 0.0);
109 final double[] bias2 = new double[3];
110 calibrator.getInitialBias(bias2);
111 assertArrayEquals(bias1, bias2, 0.0);
112 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
113 assertEquals(b1, new Matrix(3, 1));
114 final Matrix b2 = new Matrix(3, 1);
115 calibrator.getInitialBiasAsMatrix(b2);
116 assertEquals(b1, b2);
117 final Matrix ma1 = calibrator.getInitialMa();
118 assertEquals(ma1, new Matrix(3, 3));
119 final Matrix ma2 = new Matrix(3, 3);
120 calibrator.getInitialMa(ma2);
121 assertEquals(ma1, ma2);
122 assertNull(calibrator.getEcefPosition());
123 assertNull(calibrator.getNedPosition());
124 assertFalse(calibrator.getNedPosition(null));
125 assertNull(calibrator.getMeasurements());
126 assertFalse(calibrator.isCommonAxisUsed());
127 assertNull(calibrator.getListener());
128 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
129 assertFalse(calibrator.isReady());
130 assertFalse(calibrator.isRunning());
131 assertNull(calibrator.getEstimatedBiases());
132 assertFalse(calibrator.getEstimatedBiases(null));
133 assertNull(calibrator.getEstimatedBiasesAsMatrix());
134 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
135 assertNull(calibrator.getEstimatedBiasFx());
136 assertNull(calibrator.getEstimatedBiasFy());
137 assertNull(calibrator.getEstimatedBiasFz());
138 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
139 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
140 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
141 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
142 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
143 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
144 assertNull(calibrator.getEstimatedMa());
145 assertNull(calibrator.getEstimatedSx());
146 assertNull(calibrator.getEstimatedSy());
147 assertNull(calibrator.getEstimatedSz());
148 assertNull(calibrator.getEstimatedMxy());
149 assertNull(calibrator.getEstimatedMxz());
150 assertNull(calibrator.getEstimatedMyx());
151 assertNull(calibrator.getEstimatedMyz());
152 assertNull(calibrator.getEstimatedMzx());
153 assertNull(calibrator.getEstimatedMzy());
154 assertNull(calibrator.getEstimatedCovariance());
155 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
156 }
157
158 @Test
159 public void testConstructor2() throws WrongSizeException {
160 final KnownPositionAccelerometerCalibrator calibrator =
161 new KnownPositionAccelerometerCalibrator(this);
162
163
164 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
165 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
166 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
167 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
168 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
169 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
170 final Acceleration bx2 = new Acceleration(0.0,
171 AccelerationUnit.FEET_PER_SQUARED_SECOND);
172 calibrator.getInitialBiasXAsAcceleration(bx2);
173 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
174 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
175 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
176 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
177 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
178 final Acceleration by2 = new Acceleration(0.0,
179 AccelerationUnit.FEET_PER_SQUARED_SECOND);
180 calibrator.getInitialBiasYAsAcceleration(by2);
181 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
182 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
183 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
184 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
185 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
186 final Acceleration bz2 = new Acceleration(0.0,
187 AccelerationUnit.FEET_PER_SQUARED_SECOND);
188 calibrator.getInitialBiasZAsAcceleration(bz2);
189 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
190 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
191 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
192 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
193 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
194 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
195 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
196 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
197 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
198 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
199 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
200 final double[] bias1 = calibrator.getInitialBias();
201 assertArrayEquals(bias1, new double[3], 0.0);
202 final double[] bias2 = new double[3];
203 calibrator.getInitialBias(bias2);
204 assertArrayEquals(bias1, bias2, 0.0);
205 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
206 assertEquals(b1, new Matrix(3, 1));
207 final Matrix b2 = new Matrix(3, 1);
208 calibrator.getInitialBiasAsMatrix(b2);
209 assertEquals(b1, b2);
210 final Matrix ma1 = calibrator.getInitialMa();
211 assertEquals(ma1, new Matrix(3, 3));
212 final Matrix ma2 = new Matrix(3, 3);
213 calibrator.getInitialMa(ma2);
214 assertEquals(ma1, ma2);
215 assertNull(calibrator.getEcefPosition());
216 assertNull(calibrator.getNedPosition());
217 assertFalse(calibrator.getNedPosition(null));
218 assertNull(calibrator.getMeasurements());
219 assertFalse(calibrator.isCommonAxisUsed());
220 assertSame(calibrator.getListener(), this);
221 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
222 assertFalse(calibrator.isReady());
223 assertFalse(calibrator.isRunning());
224 assertNull(calibrator.getEstimatedBiases());
225 assertFalse(calibrator.getEstimatedBiases(null));
226 assertNull(calibrator.getEstimatedBiasesAsMatrix());
227 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
228 assertNull(calibrator.getEstimatedBiasFx());
229 assertNull(calibrator.getEstimatedBiasFy());
230 assertNull(calibrator.getEstimatedBiasFz());
231 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
232 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
233 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
234 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
235 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
236 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
237 assertNull(calibrator.getEstimatedMa());
238 assertNull(calibrator.getEstimatedSx());
239 assertNull(calibrator.getEstimatedSy());
240 assertNull(calibrator.getEstimatedSz());
241 assertNull(calibrator.getEstimatedMxy());
242 assertNull(calibrator.getEstimatedMxz());
243 assertNull(calibrator.getEstimatedMyx());
244 assertNull(calibrator.getEstimatedMyz());
245 assertNull(calibrator.getEstimatedMzx());
246 assertNull(calibrator.getEstimatedMzy());
247 assertNull(calibrator.getEstimatedCovariance());
248 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
249 }
250
251 @Test
252 public void testConstructor3() throws WrongSizeException {
253 final Collection<StandardDeviationBodyKinematics> measurements =
254 Collections.emptyList();
255 final KnownPositionAccelerometerCalibrator calibrator =
256 new KnownPositionAccelerometerCalibrator(measurements);
257
258
259 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
260 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
261 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
262 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
263 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
264 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
265 final Acceleration bx2 = new Acceleration(0.0,
266 AccelerationUnit.FEET_PER_SQUARED_SECOND);
267 calibrator.getInitialBiasXAsAcceleration(bx2);
268 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
269 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
270 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
271 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
272 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
273 final Acceleration by2 = new Acceleration(0.0,
274 AccelerationUnit.FEET_PER_SQUARED_SECOND);
275 calibrator.getInitialBiasYAsAcceleration(by2);
276 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
277 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
278 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
279 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
280 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
281 final Acceleration bz2 = new Acceleration(0.0,
282 AccelerationUnit.FEET_PER_SQUARED_SECOND);
283 calibrator.getInitialBiasZAsAcceleration(bz2);
284 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
285 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
286 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
287 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
288 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
289 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
290 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
291 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
292 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
293 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
294 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
295 final double[] bias1 = calibrator.getInitialBias();
296 assertArrayEquals(bias1, new double[3], 0.0);
297 final double[] bias2 = new double[3];
298 calibrator.getInitialBias(bias2);
299 assertArrayEquals(bias1, bias2, 0.0);
300 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
301 assertEquals(b1, new Matrix(3, 1));
302 final Matrix b2 = new Matrix(3, 1);
303 calibrator.getInitialBiasAsMatrix(b2);
304 assertEquals(b1, b2);
305 final Matrix ma1 = calibrator.getInitialMa();
306 assertEquals(ma1, new Matrix(3, 3));
307 final Matrix ma2 = new Matrix(3, 3);
308 calibrator.getInitialMa(ma2);
309 assertEquals(ma1, ma2);
310 assertNull(calibrator.getEcefPosition());
311 assertNull(calibrator.getNedPosition());
312 assertFalse(calibrator.getNedPosition(null));
313 assertSame(calibrator.getMeasurements(), measurements);
314 assertFalse(calibrator.isCommonAxisUsed());
315 assertNull(calibrator.getListener());
316 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
317 assertFalse(calibrator.isReady());
318 assertFalse(calibrator.isRunning());
319 assertNull(calibrator.getEstimatedBiases());
320 assertFalse(calibrator.getEstimatedBiases(null));
321 assertNull(calibrator.getEstimatedBiasesAsMatrix());
322 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
323 assertNull(calibrator.getEstimatedBiasFx());
324 assertNull(calibrator.getEstimatedBiasFy());
325 assertNull(calibrator.getEstimatedBiasFz());
326 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
327 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
328 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
329 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
330 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
331 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
332 assertNull(calibrator.getEstimatedMa());
333 assertNull(calibrator.getEstimatedSx());
334 assertNull(calibrator.getEstimatedSy());
335 assertNull(calibrator.getEstimatedSz());
336 assertNull(calibrator.getEstimatedMxy());
337 assertNull(calibrator.getEstimatedMxz());
338 assertNull(calibrator.getEstimatedMyx());
339 assertNull(calibrator.getEstimatedMyz());
340 assertNull(calibrator.getEstimatedMzx());
341 assertNull(calibrator.getEstimatedMzy());
342 assertNull(calibrator.getEstimatedCovariance());
343 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
344 }
345
346 @Test
347 public void testConstructor4() throws WrongSizeException {
348 final Collection<StandardDeviationBodyKinematics> measurements =
349 Collections.emptyList();
350 final KnownPositionAccelerometerCalibrator calibrator =
351 new KnownPositionAccelerometerCalibrator(measurements, this);
352
353
354 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
355 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
356 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
357 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
358 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
359 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
360 final Acceleration bx2 = new Acceleration(0.0,
361 AccelerationUnit.FEET_PER_SQUARED_SECOND);
362 calibrator.getInitialBiasXAsAcceleration(bx2);
363 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
364 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
365 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
366 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
367 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
368 final Acceleration by2 = new Acceleration(0.0,
369 AccelerationUnit.FEET_PER_SQUARED_SECOND);
370 calibrator.getInitialBiasYAsAcceleration(by2);
371 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
372 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
373 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
374 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
375 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
376 final Acceleration bz2 = new Acceleration(0.0,
377 AccelerationUnit.FEET_PER_SQUARED_SECOND);
378 calibrator.getInitialBiasZAsAcceleration(bz2);
379 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
380 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
381 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
382 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
383 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
384 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
385 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
386 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
387 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
388 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
389 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
390 final double[] bias1 = calibrator.getInitialBias();
391 assertArrayEquals(bias1, new double[3], 0.0);
392 final double[] bias2 = new double[3];
393 calibrator.getInitialBias(bias2);
394 assertArrayEquals(bias1, bias2, 0.0);
395 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
396 assertEquals(b1, new Matrix(3, 1));
397 final Matrix b2 = new Matrix(3, 1);
398 calibrator.getInitialBiasAsMatrix(b2);
399 assertEquals(b1, b2);
400 final Matrix ma1 = calibrator.getInitialMa();
401 assertEquals(ma1, new Matrix(3, 3));
402 final Matrix ma2 = new Matrix(3, 3);
403 calibrator.getInitialMa(ma2);
404 assertEquals(ma1, ma2);
405 assertNull(calibrator.getEcefPosition());
406 assertNull(calibrator.getNedPosition());
407 assertFalse(calibrator.getNedPosition(null));
408 assertSame(calibrator.getMeasurements(), measurements);
409 assertFalse(calibrator.isCommonAxisUsed());
410 assertSame(calibrator.getListener(), this);
411 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
412 assertFalse(calibrator.isReady());
413 assertFalse(calibrator.isRunning());
414 assertNull(calibrator.getEstimatedBiases());
415 assertFalse(calibrator.getEstimatedBiases(null));
416 assertNull(calibrator.getEstimatedBiasesAsMatrix());
417 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
418 assertNull(calibrator.getEstimatedBiasFx());
419 assertNull(calibrator.getEstimatedBiasFy());
420 assertNull(calibrator.getEstimatedBiasFz());
421 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
422 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
423 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
424 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
425 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
426 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
427 assertNull(calibrator.getEstimatedMa());
428 assertNull(calibrator.getEstimatedSx());
429 assertNull(calibrator.getEstimatedSy());
430 assertNull(calibrator.getEstimatedSz());
431 assertNull(calibrator.getEstimatedMxy());
432 assertNull(calibrator.getEstimatedMxz());
433 assertNull(calibrator.getEstimatedMyx());
434 assertNull(calibrator.getEstimatedMyz());
435 assertNull(calibrator.getEstimatedMzx());
436 assertNull(calibrator.getEstimatedMzy());
437 assertNull(calibrator.getEstimatedCovariance());
438 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
439 }
440
441 @Test
442 public void testConstructor5() throws WrongSizeException {
443 final KnownPositionAccelerometerCalibrator calibrator =
444 new KnownPositionAccelerometerCalibrator(true);
445
446
447 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
448 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
449 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
450 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
451 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
452 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
453 final Acceleration bx2 = new Acceleration(0.0,
454 AccelerationUnit.FEET_PER_SQUARED_SECOND);
455 calibrator.getInitialBiasXAsAcceleration(bx2);
456 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
457 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
458 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
459 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
460 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
461 final Acceleration by2 = new Acceleration(0.0,
462 AccelerationUnit.FEET_PER_SQUARED_SECOND);
463 calibrator.getInitialBiasYAsAcceleration(by2);
464 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
465 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
466 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
467 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
468 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
469 final Acceleration bz2 = new Acceleration(0.0,
470 AccelerationUnit.FEET_PER_SQUARED_SECOND);
471 calibrator.getInitialBiasZAsAcceleration(bz2);
472 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
473 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
474 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
475 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
476 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
477 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
478 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
479 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
480 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
481 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
482 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
483 final double[] bias1 = calibrator.getInitialBias();
484 assertArrayEquals(bias1, new double[3], 0.0);
485 final double[] bias2 = new double[3];
486 calibrator.getInitialBias(bias2);
487 assertArrayEquals(bias1, bias2, 0.0);
488 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
489 assertEquals(b1, new Matrix(3, 1));
490 final Matrix b2 = new Matrix(3, 1);
491 calibrator.getInitialBiasAsMatrix(b2);
492 assertEquals(b1, b2);
493 final Matrix ma1 = calibrator.getInitialMa();
494 assertEquals(ma1, new Matrix(3, 3));
495 final Matrix ma2 = new Matrix(3, 3);
496 calibrator.getInitialMa(ma2);
497 assertEquals(ma1, ma2);
498 assertNull(calibrator.getEcefPosition());
499 assertNull(calibrator.getNedPosition());
500 assertFalse(calibrator.getNedPosition(null));
501 assertNull(calibrator.getMeasurements());
502 assertTrue(calibrator.isCommonAxisUsed());
503 assertNull(calibrator.getListener());
504 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
505 assertFalse(calibrator.isReady());
506 assertFalse(calibrator.isRunning());
507 assertNull(calibrator.getEstimatedBiases());
508 assertFalse(calibrator.getEstimatedBiases(null));
509 assertNull(calibrator.getEstimatedBiasesAsMatrix());
510 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
511 assertNull(calibrator.getEstimatedBiasFx());
512 assertNull(calibrator.getEstimatedBiasFy());
513 assertNull(calibrator.getEstimatedBiasFz());
514 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
515 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
516 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
517 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
518 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
519 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
520 assertNull(calibrator.getEstimatedMa());
521 assertNull(calibrator.getEstimatedSx());
522 assertNull(calibrator.getEstimatedSy());
523 assertNull(calibrator.getEstimatedSz());
524 assertNull(calibrator.getEstimatedMxy());
525 assertNull(calibrator.getEstimatedMxz());
526 assertNull(calibrator.getEstimatedMyx());
527 assertNull(calibrator.getEstimatedMyz());
528 assertNull(calibrator.getEstimatedMzx());
529 assertNull(calibrator.getEstimatedMzy());
530 assertNull(calibrator.getEstimatedCovariance());
531 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
532 }
533
534 @Test
535 public void testConstructor6() throws WrongSizeException {
536 final KnownPositionAccelerometerCalibrator calibrator =
537 new KnownPositionAccelerometerCalibrator(true,
538 this);
539
540
541 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
542 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
543 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
544 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
545 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
546 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
547 final Acceleration bx2 = new Acceleration(0.0,
548 AccelerationUnit.FEET_PER_SQUARED_SECOND);
549 calibrator.getInitialBiasXAsAcceleration(bx2);
550 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
551 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
552 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
553 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
554 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
555 final Acceleration by2 = new Acceleration(0.0,
556 AccelerationUnit.FEET_PER_SQUARED_SECOND);
557 calibrator.getInitialBiasYAsAcceleration(by2);
558 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
559 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
560 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
561 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
562 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
563 final Acceleration bz2 = new Acceleration(0.0,
564 AccelerationUnit.FEET_PER_SQUARED_SECOND);
565 calibrator.getInitialBiasZAsAcceleration(bz2);
566 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
567 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
568 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
569 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
570 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
571 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
572 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
573 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
574 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
575 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
576 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
577 final double[] bias1 = calibrator.getInitialBias();
578 assertArrayEquals(bias1, new double[3], 0.0);
579 final double[] bias2 = new double[3];
580 calibrator.getInitialBias(bias2);
581 assertArrayEquals(bias1, bias2, 0.0);
582 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
583 assertEquals(b1, new Matrix(3, 1));
584 final Matrix b2 = new Matrix(3, 1);
585 calibrator.getInitialBiasAsMatrix(b2);
586 assertEquals(b1, b2);
587 final Matrix ma1 = calibrator.getInitialMa();
588 assertEquals(ma1, new Matrix(3, 3));
589 final Matrix ma2 = new Matrix(3, 3);
590 calibrator.getInitialMa(ma2);
591 assertEquals(ma1, ma2);
592 assertNull(calibrator.getEcefPosition());
593 assertNull(calibrator.getNedPosition());
594 assertFalse(calibrator.getNedPosition(null));
595 assertNull(calibrator.getMeasurements());
596 assertTrue(calibrator.isCommonAxisUsed());
597 assertSame(calibrator.getListener(), this);
598 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
599 assertFalse(calibrator.isReady());
600 assertFalse(calibrator.isRunning());
601 assertNull(calibrator.getEstimatedBiases());
602 assertFalse(calibrator.getEstimatedBiases(null));
603 assertNull(calibrator.getEstimatedBiasesAsMatrix());
604 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
605 assertNull(calibrator.getEstimatedBiasFx());
606 assertNull(calibrator.getEstimatedBiasFy());
607 assertNull(calibrator.getEstimatedBiasFz());
608 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
609 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
610 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
611 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
612 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
613 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
614 assertNull(calibrator.getEstimatedMa());
615 assertNull(calibrator.getEstimatedSx());
616 assertNull(calibrator.getEstimatedSy());
617 assertNull(calibrator.getEstimatedSz());
618 assertNull(calibrator.getEstimatedMxy());
619 assertNull(calibrator.getEstimatedMxz());
620 assertNull(calibrator.getEstimatedMyx());
621 assertNull(calibrator.getEstimatedMyz());
622 assertNull(calibrator.getEstimatedMzx());
623 assertNull(calibrator.getEstimatedMzy());
624 assertNull(calibrator.getEstimatedCovariance());
625 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
626 }
627
628 @Test
629 public void testConstructor7() throws WrongSizeException {
630 final Collection<StandardDeviationBodyKinematics> measurements =
631 Collections.emptyList();
632 final KnownPositionAccelerometerCalibrator calibrator =
633 new KnownPositionAccelerometerCalibrator(measurements,
634 true);
635
636
637 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
638 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
639 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
640 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
641 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
642 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
643 final Acceleration bx2 = new Acceleration(0.0,
644 AccelerationUnit.FEET_PER_SQUARED_SECOND);
645 calibrator.getInitialBiasXAsAcceleration(bx2);
646 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
647 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
648 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
649 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
650 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
651 final Acceleration by2 = new Acceleration(0.0,
652 AccelerationUnit.FEET_PER_SQUARED_SECOND);
653 calibrator.getInitialBiasYAsAcceleration(by2);
654 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
655 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
656 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
657 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
658 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
659 final Acceleration bz2 = new Acceleration(0.0,
660 AccelerationUnit.FEET_PER_SQUARED_SECOND);
661 calibrator.getInitialBiasZAsAcceleration(bz2);
662 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
663 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
664 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
665 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
666 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
667 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
668 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
669 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
670 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
671 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
672 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
673 final double[] bias1 = calibrator.getInitialBias();
674 assertArrayEquals(bias1, new double[3], 0.0);
675 final double[] bias2 = new double[3];
676 calibrator.getInitialBias(bias2);
677 assertArrayEquals(bias1, bias2, 0.0);
678 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
679 assertEquals(b1, new Matrix(3, 1));
680 final Matrix b2 = new Matrix(3, 1);
681 calibrator.getInitialBiasAsMatrix(b2);
682 assertEquals(b1, b2);
683 final Matrix ma1 = calibrator.getInitialMa();
684 assertEquals(ma1, new Matrix(3, 3));
685 final Matrix ma2 = new Matrix(3, 3);
686 calibrator.getInitialMa(ma2);
687 assertEquals(ma1, ma2);
688 assertNull(calibrator.getEcefPosition());
689 assertNull(calibrator.getNedPosition());
690 assertFalse(calibrator.getNedPosition(null));
691 assertSame(calibrator.getMeasurements(), measurements);
692 assertTrue(calibrator.isCommonAxisUsed());
693 assertNull(calibrator.getListener());
694 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
695 assertFalse(calibrator.isReady());
696 assertFalse(calibrator.isRunning());
697 assertNull(calibrator.getEstimatedBiases());
698 assertFalse(calibrator.getEstimatedBiases(null));
699 assertNull(calibrator.getEstimatedBiasesAsMatrix());
700 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
701 assertNull(calibrator.getEstimatedBiasFx());
702 assertNull(calibrator.getEstimatedBiasFy());
703 assertNull(calibrator.getEstimatedBiasFz());
704 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
705 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
706 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
707 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
708 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
709 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
710 assertNull(calibrator.getEstimatedMa());
711 assertNull(calibrator.getEstimatedSx());
712 assertNull(calibrator.getEstimatedSy());
713 assertNull(calibrator.getEstimatedSz());
714 assertNull(calibrator.getEstimatedMxy());
715 assertNull(calibrator.getEstimatedMxz());
716 assertNull(calibrator.getEstimatedMyx());
717 assertNull(calibrator.getEstimatedMyz());
718 assertNull(calibrator.getEstimatedMzx());
719 assertNull(calibrator.getEstimatedMzy());
720 assertNull(calibrator.getEstimatedCovariance());
721 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
722 }
723
724 @Test
725 public void testConstructor8() throws WrongSizeException {
726 final Collection<StandardDeviationBodyKinematics> measurements =
727 Collections.emptyList();
728 final KnownPositionAccelerometerCalibrator calibrator =
729 new KnownPositionAccelerometerCalibrator(measurements,
730 true, this);
731
732
733 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
734 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
735 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
736 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
737 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
738 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
739 final Acceleration bx2 = new Acceleration(0.0,
740 AccelerationUnit.FEET_PER_SQUARED_SECOND);
741 calibrator.getInitialBiasXAsAcceleration(bx2);
742 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
743 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
744 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
745 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
746 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
747 final Acceleration by2 = new Acceleration(0.0,
748 AccelerationUnit.FEET_PER_SQUARED_SECOND);
749 calibrator.getInitialBiasYAsAcceleration(by2);
750 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
751 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
752 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
753 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
754 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
755 final Acceleration bz2 = new Acceleration(0.0,
756 AccelerationUnit.FEET_PER_SQUARED_SECOND);
757 calibrator.getInitialBiasZAsAcceleration(bz2);
758 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
759 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
760 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
761 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
762 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
763 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
764 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
765 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
766 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
767 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
768 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
769 final double[] bias1 = calibrator.getInitialBias();
770 assertArrayEquals(bias1, new double[3], 0.0);
771 final double[] bias2 = new double[3];
772 calibrator.getInitialBias(bias2);
773 assertArrayEquals(bias1, bias2, 0.0);
774 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
775 assertEquals(b1, new Matrix(3, 1));
776 final Matrix b2 = new Matrix(3, 1);
777 calibrator.getInitialBiasAsMatrix(b2);
778 assertEquals(b1, b2);
779 final Matrix ma1 = calibrator.getInitialMa();
780 assertEquals(ma1, new Matrix(3, 3));
781 final Matrix ma2 = new Matrix(3, 3);
782 calibrator.getInitialMa(ma2);
783 assertEquals(ma1, ma2);
784 assertNull(calibrator.getEcefPosition());
785 assertNull(calibrator.getNedPosition());
786 assertFalse(calibrator.getNedPosition(null));
787 assertSame(calibrator.getMeasurements(), measurements);
788 assertTrue(calibrator.isCommonAxisUsed());
789 assertSame(calibrator.getListener(), this);
790 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
791 assertFalse(calibrator.isReady());
792 assertFalse(calibrator.isRunning());
793 assertNull(calibrator.getEstimatedBiases());
794 assertFalse(calibrator.getEstimatedBiases(null));
795 assertNull(calibrator.getEstimatedBiasesAsMatrix());
796 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
797 assertNull(calibrator.getEstimatedBiasFx());
798 assertNull(calibrator.getEstimatedBiasFy());
799 assertNull(calibrator.getEstimatedBiasFz());
800 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
801 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
802 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
803 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
804 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
805 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
806 assertNull(calibrator.getEstimatedMa());
807 assertNull(calibrator.getEstimatedSx());
808 assertNull(calibrator.getEstimatedSy());
809 assertNull(calibrator.getEstimatedSz());
810 assertNull(calibrator.getEstimatedMxy());
811 assertNull(calibrator.getEstimatedMxz());
812 assertNull(calibrator.getEstimatedMyx());
813 assertNull(calibrator.getEstimatedMyz());
814 assertNull(calibrator.getEstimatedMzx());
815 assertNull(calibrator.getEstimatedMzy());
816 assertNull(calibrator.getEstimatedCovariance());
817 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
818 }
819
820 @Test
821 public void testConstructor9() throws WrongSizeException {
822 final Matrix ba = generateBa();
823 final double biasX = ba.getElementAtIndex(0);
824 final double biasY = ba.getElementAtIndex(1);
825 final double biasZ = ba.getElementAtIndex(2);
826
827 final KnownPositionAccelerometerCalibrator calibrator =
828 new KnownPositionAccelerometerCalibrator(biasX, biasY, biasZ);
829
830
831 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
832 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
833 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
834 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
835 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
836 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
837 final Acceleration bx2 = new Acceleration(0.0,
838 AccelerationUnit.FEET_PER_SQUARED_SECOND);
839 calibrator.getInitialBiasXAsAcceleration(bx2);
840 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
841 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
842 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
843 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
844 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
845 final Acceleration by2 = new Acceleration(0.0,
846 AccelerationUnit.FEET_PER_SQUARED_SECOND);
847 calibrator.getInitialBiasYAsAcceleration(by2);
848 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
849 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
850 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
851 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
852 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
853 final Acceleration bz2 = new Acceleration(0.0,
854 AccelerationUnit.FEET_PER_SQUARED_SECOND);
855 calibrator.getInitialBiasZAsAcceleration(bz2);
856 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
857 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
858 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
859 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
860 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
861 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
862 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
863 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
864 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
865 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
866 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
867 final double[] bias1 = calibrator.getInitialBias();
868 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
869 final double[] bias2 = new double[3];
870 calibrator.getInitialBias(bias2);
871 assertArrayEquals(bias1, bias2, 0.0);
872 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
873 assertEquals(b1, ba);
874 final Matrix b2 = new Matrix(3, 1);
875 calibrator.getInitialBiasAsMatrix(b2);
876 assertEquals(b1, b2);
877 final Matrix ma1 = calibrator.getInitialMa();
878 assertEquals(ma1, new Matrix(3, 3));
879 final Matrix ma2 = new Matrix(3, 3);
880 calibrator.getInitialMa(ma2);
881 assertEquals(ma1, ma2);
882 assertNull(calibrator.getEcefPosition());
883 assertNull(calibrator.getNedPosition());
884 assertFalse(calibrator.getNedPosition(null));
885 assertNull(calibrator.getMeasurements());
886 assertFalse(calibrator.isCommonAxisUsed());
887 assertNull(calibrator.getListener());
888 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
889 assertFalse(calibrator.isReady());
890 assertFalse(calibrator.isRunning());
891 assertNull(calibrator.getEstimatedBiases());
892 assertFalse(calibrator.getEstimatedBiases(null));
893 assertNull(calibrator.getEstimatedBiasesAsMatrix());
894 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
895 assertNull(calibrator.getEstimatedBiasFx());
896 assertNull(calibrator.getEstimatedBiasFy());
897 assertNull(calibrator.getEstimatedBiasFz());
898 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
899 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
900 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
901 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
902 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
903 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
904 assertNull(calibrator.getEstimatedMa());
905 assertNull(calibrator.getEstimatedSx());
906 assertNull(calibrator.getEstimatedSy());
907 assertNull(calibrator.getEstimatedSz());
908 assertNull(calibrator.getEstimatedMxy());
909 assertNull(calibrator.getEstimatedMxz());
910 assertNull(calibrator.getEstimatedMyx());
911 assertNull(calibrator.getEstimatedMyz());
912 assertNull(calibrator.getEstimatedMzx());
913 assertNull(calibrator.getEstimatedMzy());
914 assertNull(calibrator.getEstimatedCovariance());
915 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
916 }
917
918 @Test
919 public void testConstructor10() throws WrongSizeException {
920 final Matrix ba = generateBa();
921 final double biasX = ba.getElementAtIndex(0);
922 final double biasY = ba.getElementAtIndex(1);
923 final double biasZ = ba.getElementAtIndex(2);
924
925 final KnownPositionAccelerometerCalibrator calibrator =
926 new KnownPositionAccelerometerCalibrator(biasX, biasY, biasZ,
927 this);
928
929
930 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
931 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
932 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
933 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
934 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
935 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
936 final Acceleration bx2 = new Acceleration(0.0,
937 AccelerationUnit.FEET_PER_SQUARED_SECOND);
938 calibrator.getInitialBiasXAsAcceleration(bx2);
939 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
940 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
941 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
942 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
943 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
944 final Acceleration by2 = new Acceleration(0.0,
945 AccelerationUnit.FEET_PER_SQUARED_SECOND);
946 calibrator.getInitialBiasYAsAcceleration(by2);
947 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
948 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
949 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
950 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
951 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
952 final Acceleration bz2 = new Acceleration(0.0,
953 AccelerationUnit.FEET_PER_SQUARED_SECOND);
954 calibrator.getInitialBiasZAsAcceleration(bz2);
955 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
956 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
957 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
958 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
959 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
960 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
961 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
962 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
963 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
964 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
965 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
966 final double[] bias1 = calibrator.getInitialBias();
967 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
968 final double[] bias2 = new double[3];
969 calibrator.getInitialBias(bias2);
970 assertArrayEquals(bias1, bias2, 0.0);
971 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
972 assertEquals(b1, ba);
973 final Matrix b2 = new Matrix(3, 1);
974 calibrator.getInitialBiasAsMatrix(b2);
975 assertEquals(b1, b2);
976 final Matrix ma1 = calibrator.getInitialMa();
977 assertEquals(ma1, new Matrix(3, 3));
978 final Matrix ma2 = new Matrix(3, 3);
979 calibrator.getInitialMa(ma2);
980 assertEquals(ma1, ma2);
981 assertNull(calibrator.getEcefPosition());
982 assertNull(calibrator.getNedPosition());
983 assertFalse(calibrator.getNedPosition(null));
984 assertNull(calibrator.getMeasurements());
985 assertFalse(calibrator.isCommonAxisUsed());
986 assertSame(calibrator.getListener(), this);
987 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
988 assertFalse(calibrator.isReady());
989 assertFalse(calibrator.isRunning());
990 assertNull(calibrator.getEstimatedBiases());
991 assertFalse(calibrator.getEstimatedBiases(null));
992 assertNull(calibrator.getEstimatedBiasesAsMatrix());
993 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
994 assertNull(calibrator.getEstimatedBiasFx());
995 assertNull(calibrator.getEstimatedBiasFy());
996 assertNull(calibrator.getEstimatedBiasFz());
997 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
998 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
999 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1000 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1001 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1002 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1003 assertNull(calibrator.getEstimatedMa());
1004 assertNull(calibrator.getEstimatedSx());
1005 assertNull(calibrator.getEstimatedSy());
1006 assertNull(calibrator.getEstimatedSz());
1007 assertNull(calibrator.getEstimatedMxy());
1008 assertNull(calibrator.getEstimatedMxz());
1009 assertNull(calibrator.getEstimatedMyx());
1010 assertNull(calibrator.getEstimatedMyz());
1011 assertNull(calibrator.getEstimatedMzx());
1012 assertNull(calibrator.getEstimatedMzy());
1013 assertNull(calibrator.getEstimatedCovariance());
1014 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1015 }
1016
1017 @Test
1018 public void testConstructor11() throws WrongSizeException {
1019 final Collection<StandardDeviationBodyKinematics> measurements =
1020 Collections.emptyList();
1021
1022 final Matrix ba = generateBa();
1023 final double biasX = ba.getElementAtIndex(0);
1024 final double biasY = ba.getElementAtIndex(1);
1025 final double biasZ = ba.getElementAtIndex(2);
1026
1027 final KnownPositionAccelerometerCalibrator calibrator =
1028 new KnownPositionAccelerometerCalibrator(measurements,
1029 biasX, biasY, biasZ);
1030
1031
1032 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1033 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1034 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1035 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1036 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1037 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1038 final Acceleration bx2 = new Acceleration(0.0,
1039 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1040 calibrator.getInitialBiasXAsAcceleration(bx2);
1041 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1042 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1043 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1044 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1045 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1046 final Acceleration by2 = new Acceleration(0.0,
1047 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1048 calibrator.getInitialBiasYAsAcceleration(by2);
1049 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1050 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1051 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1052 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1053 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1054 final Acceleration bz2 = new Acceleration(0.0,
1055 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1056 calibrator.getInitialBiasZAsAcceleration(bz2);
1057 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1058 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1059 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1060 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1061 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1062 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1063 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1064 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1065 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1066 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1067 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1068 final double[] bias1 = calibrator.getInitialBias();
1069 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
1070 final double[] bias2 = new double[3];
1071 calibrator.getInitialBias(bias2);
1072 assertArrayEquals(bias1, bias2, 0.0);
1073 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1074 assertEquals(b1, ba);
1075 final Matrix b2 = new Matrix(3, 1);
1076 calibrator.getInitialBiasAsMatrix(b2);
1077 assertEquals(b1, b2);
1078 final Matrix ma1 = calibrator.getInitialMa();
1079 assertEquals(ma1, new Matrix(3, 3));
1080 final Matrix ma2 = new Matrix(3, 3);
1081 calibrator.getInitialMa(ma2);
1082 assertEquals(ma1, ma2);
1083 assertNull(calibrator.getEcefPosition());
1084 assertNull(calibrator.getNedPosition());
1085 assertFalse(calibrator.getNedPosition(null));
1086 assertSame(calibrator.getMeasurements(), measurements);
1087 assertFalse(calibrator.isCommonAxisUsed());
1088 assertNull(calibrator.getListener());
1089 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1090 assertFalse(calibrator.isReady());
1091 assertFalse(calibrator.isRunning());
1092 assertNull(calibrator.getEstimatedBiases());
1093 assertFalse(calibrator.getEstimatedBiases(null));
1094 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1095 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1096 assertNull(calibrator.getEstimatedBiasFx());
1097 assertNull(calibrator.getEstimatedBiasFy());
1098 assertNull(calibrator.getEstimatedBiasFz());
1099 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1100 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1101 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1102 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1103 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1104 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1105 assertNull(calibrator.getEstimatedMa());
1106 assertNull(calibrator.getEstimatedSx());
1107 assertNull(calibrator.getEstimatedSy());
1108 assertNull(calibrator.getEstimatedSz());
1109 assertNull(calibrator.getEstimatedMxy());
1110 assertNull(calibrator.getEstimatedMxz());
1111 assertNull(calibrator.getEstimatedMyx());
1112 assertNull(calibrator.getEstimatedMyz());
1113 assertNull(calibrator.getEstimatedMzx());
1114 assertNull(calibrator.getEstimatedMzy());
1115 assertNull(calibrator.getEstimatedCovariance());
1116 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1117 }
1118
1119 @Test
1120 public void testConstructor12() throws WrongSizeException {
1121 final Collection<StandardDeviationBodyKinematics> measurements =
1122 Collections.emptyList();
1123
1124 final Matrix ba = generateBa();
1125 final double biasX = ba.getElementAtIndex(0);
1126 final double biasY = ba.getElementAtIndex(1);
1127 final double biasZ = ba.getElementAtIndex(2);
1128
1129 final KnownPositionAccelerometerCalibrator calibrator =
1130 new KnownPositionAccelerometerCalibrator(measurements,
1131 biasX, biasY, biasZ, this);
1132
1133
1134 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1135 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1136 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1137 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1138 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1139 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1140 final Acceleration bx2 = new Acceleration(0.0,
1141 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1142 calibrator.getInitialBiasXAsAcceleration(bx2);
1143 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1144 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1145 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1146 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1147 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1148 final Acceleration by2 = new Acceleration(0.0,
1149 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1150 calibrator.getInitialBiasYAsAcceleration(by2);
1151 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1152 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1153 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1154 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1155 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1156 final Acceleration bz2 = new Acceleration(0.0,
1157 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1158 calibrator.getInitialBiasZAsAcceleration(bz2);
1159 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1160 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1161 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1162 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1163 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1164 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1165 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1166 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1167 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1168 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1169 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1170 final double[] bias1 = calibrator.getInitialBias();
1171 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
1172 final double[] bias2 = new double[3];
1173 calibrator.getInitialBias(bias2);
1174 assertArrayEquals(bias1, bias2, 0.0);
1175 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1176 assertEquals(b1, ba);
1177 final Matrix b2 = new Matrix(3, 1);
1178 calibrator.getInitialBiasAsMatrix(b2);
1179 assertEquals(b1, b2);
1180 final Matrix ma1 = calibrator.getInitialMa();
1181 assertEquals(ma1, new Matrix(3, 3));
1182 final Matrix ma2 = new Matrix(3, 3);
1183 calibrator.getInitialMa(ma2);
1184 assertEquals(ma1, ma2);
1185 assertNull(calibrator.getEcefPosition());
1186 assertNull(calibrator.getNedPosition());
1187 assertFalse(calibrator.getNedPosition(null));
1188 assertSame(calibrator.getMeasurements(), measurements);
1189 assertFalse(calibrator.isCommonAxisUsed());
1190 assertSame(calibrator.getListener(), this);
1191 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1192 assertFalse(calibrator.isReady());
1193 assertFalse(calibrator.isRunning());
1194 assertNull(calibrator.getEstimatedBiases());
1195 assertFalse(calibrator.getEstimatedBiases(null));
1196 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1197 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1198 assertNull(calibrator.getEstimatedBiasFx());
1199 assertNull(calibrator.getEstimatedBiasFy());
1200 assertNull(calibrator.getEstimatedBiasFz());
1201 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1202 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1203 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1204 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1205 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1206 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1207 assertNull(calibrator.getEstimatedMa());
1208 assertNull(calibrator.getEstimatedSx());
1209 assertNull(calibrator.getEstimatedSy());
1210 assertNull(calibrator.getEstimatedSz());
1211 assertNull(calibrator.getEstimatedMxy());
1212 assertNull(calibrator.getEstimatedMxz());
1213 assertNull(calibrator.getEstimatedMyx());
1214 assertNull(calibrator.getEstimatedMyz());
1215 assertNull(calibrator.getEstimatedMzx());
1216 assertNull(calibrator.getEstimatedMzy());
1217 assertNull(calibrator.getEstimatedCovariance());
1218 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1219 }
1220
1221 @Test
1222 public void testConstructor13() throws WrongSizeException {
1223 final Matrix ba = generateBa();
1224 final double biasX = ba.getElementAtIndex(0);
1225 final double biasY = ba.getElementAtIndex(1);
1226 final double biasZ = ba.getElementAtIndex(2);
1227
1228 final KnownPositionAccelerometerCalibrator calibrator =
1229 new KnownPositionAccelerometerCalibrator(true,
1230 biasX, biasY, biasZ);
1231
1232
1233 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1234 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1235 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1236 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1237 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1238 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1239 final Acceleration bx2 = new Acceleration(0.0,
1240 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1241 calibrator.getInitialBiasXAsAcceleration(bx2);
1242 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1243 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1244 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1245 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1246 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1247 final Acceleration by2 = new Acceleration(0.0,
1248 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1249 calibrator.getInitialBiasYAsAcceleration(by2);
1250 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1251 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1252 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1253 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1254 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1255 final Acceleration bz2 = new Acceleration(0.0,
1256 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1257 calibrator.getInitialBiasZAsAcceleration(bz2);
1258 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1259 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1260 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1261 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1262 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1263 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1264 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1265 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1266 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1267 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1268 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1269 final double[] bias1 = calibrator.getInitialBias();
1270 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
1271 final double[] bias2 = new double[3];
1272 calibrator.getInitialBias(bias2);
1273 assertArrayEquals(bias1, bias2, 0.0);
1274 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1275 assertEquals(b1, ba);
1276 final Matrix b2 = new Matrix(3, 1);
1277 calibrator.getInitialBiasAsMatrix(b2);
1278 assertEquals(b1, b2);
1279 final Matrix ma1 = calibrator.getInitialMa();
1280 assertEquals(ma1, new Matrix(3, 3));
1281 final Matrix ma2 = new Matrix(3, 3);
1282 calibrator.getInitialMa(ma2);
1283 assertEquals(ma1, ma2);
1284 assertNull(calibrator.getEcefPosition());
1285 assertNull(calibrator.getNedPosition());
1286 assertFalse(calibrator.getNedPosition(null));
1287 assertNull(calibrator.getMeasurements());
1288 assertTrue(calibrator.isCommonAxisUsed());
1289 assertNull(calibrator.getListener());
1290 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1291 assertFalse(calibrator.isReady());
1292 assertFalse(calibrator.isRunning());
1293 assertNull(calibrator.getEstimatedBiases());
1294 assertFalse(calibrator.getEstimatedBiases(null));
1295 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1296 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1297 assertNull(calibrator.getEstimatedBiasFx());
1298 assertNull(calibrator.getEstimatedBiasFy());
1299 assertNull(calibrator.getEstimatedBiasFz());
1300 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1301 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1302 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1303 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1304 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1305 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1306 assertNull(calibrator.getEstimatedMa());
1307 assertNull(calibrator.getEstimatedSx());
1308 assertNull(calibrator.getEstimatedSy());
1309 assertNull(calibrator.getEstimatedSz());
1310 assertNull(calibrator.getEstimatedMxy());
1311 assertNull(calibrator.getEstimatedMxz());
1312 assertNull(calibrator.getEstimatedMyx());
1313 assertNull(calibrator.getEstimatedMyz());
1314 assertNull(calibrator.getEstimatedMzx());
1315 assertNull(calibrator.getEstimatedMzy());
1316 assertNull(calibrator.getEstimatedCovariance());
1317 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1318 }
1319
1320 @Test
1321 public void testConstructor14() throws WrongSizeException {
1322 final Matrix ba = generateBa();
1323 final double biasX = ba.getElementAtIndex(0);
1324 final double biasY = ba.getElementAtIndex(1);
1325 final double biasZ = ba.getElementAtIndex(2);
1326
1327 final KnownPositionAccelerometerCalibrator calibrator =
1328 new KnownPositionAccelerometerCalibrator(true,
1329 biasX, biasY, biasZ, this);
1330
1331
1332 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1333 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1334 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1335 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1336 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1337 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1338 final Acceleration bx2 = new Acceleration(0.0,
1339 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1340 calibrator.getInitialBiasXAsAcceleration(bx2);
1341 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1342 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1343 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1344 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1345 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1346 final Acceleration by2 = new Acceleration(0.0,
1347 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1348 calibrator.getInitialBiasYAsAcceleration(by2);
1349 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1350 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1351 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1352 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1353 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1354 final Acceleration bz2 = new Acceleration(0.0,
1355 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1356 calibrator.getInitialBiasZAsAcceleration(bz2);
1357 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1358 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1359 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1360 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1361 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1362 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1363 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1364 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1365 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1366 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1367 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1368 final double[] bias1 = calibrator.getInitialBias();
1369 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
1370 final double[] bias2 = new double[3];
1371 calibrator.getInitialBias(bias2);
1372 assertArrayEquals(bias1, bias2, 0.0);
1373 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1374 assertEquals(b1, ba);
1375 final Matrix b2 = new Matrix(3, 1);
1376 calibrator.getInitialBiasAsMatrix(b2);
1377 assertEquals(b1, b2);
1378 final Matrix ma1 = calibrator.getInitialMa();
1379 assertEquals(ma1, new Matrix(3, 3));
1380 final Matrix ma2 = new Matrix(3, 3);
1381 calibrator.getInitialMa(ma2);
1382 assertEquals(ma1, ma2);
1383 assertNull(calibrator.getEcefPosition());
1384 assertNull(calibrator.getNedPosition());
1385 assertFalse(calibrator.getNedPosition(null));
1386 assertNull(calibrator.getMeasurements());
1387 assertTrue(calibrator.isCommonAxisUsed());
1388 assertSame(calibrator.getListener(), this);
1389 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1390 assertFalse(calibrator.isReady());
1391 assertFalse(calibrator.isRunning());
1392 assertNull(calibrator.getEstimatedBiases());
1393 assertFalse(calibrator.getEstimatedBiases(null));
1394 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1395 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1396 assertNull(calibrator.getEstimatedBiasFx());
1397 assertNull(calibrator.getEstimatedBiasFy());
1398 assertNull(calibrator.getEstimatedBiasFz());
1399 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1400 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1401 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1402 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1403 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1404 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1405 assertNull(calibrator.getEstimatedMa());
1406 assertNull(calibrator.getEstimatedSx());
1407 assertNull(calibrator.getEstimatedSy());
1408 assertNull(calibrator.getEstimatedSz());
1409 assertNull(calibrator.getEstimatedMxy());
1410 assertNull(calibrator.getEstimatedMxz());
1411 assertNull(calibrator.getEstimatedMyx());
1412 assertNull(calibrator.getEstimatedMyz());
1413 assertNull(calibrator.getEstimatedMzx());
1414 assertNull(calibrator.getEstimatedMzy());
1415 assertNull(calibrator.getEstimatedCovariance());
1416 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1417 }
1418
1419 @Test
1420 public void testConstructor15() throws WrongSizeException {
1421 final Collection<StandardDeviationBodyKinematics> measurements =
1422 Collections.emptyList();
1423
1424 final Matrix ba = generateBa();
1425 final double biasX = ba.getElementAtIndex(0);
1426 final double biasY = ba.getElementAtIndex(1);
1427 final double biasZ = ba.getElementAtIndex(2);
1428
1429 final KnownPositionAccelerometerCalibrator calibrator =
1430 new KnownPositionAccelerometerCalibrator(measurements,
1431 true, biasX, biasY, biasZ);
1432
1433
1434 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1435 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1436 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1437 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1438 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1439 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1440 final Acceleration bx2 = new Acceleration(0.0,
1441 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1442 calibrator.getInitialBiasXAsAcceleration(bx2);
1443 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1444 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1445 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1446 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1447 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1448 final Acceleration by2 = new Acceleration(0.0,
1449 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1450 calibrator.getInitialBiasYAsAcceleration(by2);
1451 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1452 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1453 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1454 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1455 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1456 final Acceleration bz2 = new Acceleration(0.0,
1457 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1458 calibrator.getInitialBiasZAsAcceleration(bz2);
1459 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1460 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1461 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1462 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1463 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1464 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1465 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1466 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1467 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1468 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1469 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1470 final double[] bias1 = calibrator.getInitialBias();
1471 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
1472 final double[] bias2 = new double[3];
1473 calibrator.getInitialBias(bias2);
1474 assertArrayEquals(bias1, bias2, 0.0);
1475 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1476 assertEquals(b1, ba);
1477 final Matrix b2 = new Matrix(3, 1);
1478 calibrator.getInitialBiasAsMatrix(b2);
1479 assertEquals(b1, b2);
1480 final Matrix ma1 = calibrator.getInitialMa();
1481 assertEquals(ma1, new Matrix(3, 3));
1482 final Matrix ma2 = new Matrix(3, 3);
1483 calibrator.getInitialMa(ma2);
1484 assertEquals(ma1, ma2);
1485 assertNull(calibrator.getEcefPosition());
1486 assertNull(calibrator.getNedPosition());
1487 assertFalse(calibrator.getNedPosition(null));
1488 assertSame(calibrator.getMeasurements(), measurements);
1489 assertTrue(calibrator.isCommonAxisUsed());
1490 assertNull(calibrator.getListener());
1491 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1492 assertFalse(calibrator.isReady());
1493 assertFalse(calibrator.isRunning());
1494 assertNull(calibrator.getEstimatedBiases());
1495 assertFalse(calibrator.getEstimatedBiases(null));
1496 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1497 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1498 assertNull(calibrator.getEstimatedBiasFx());
1499 assertNull(calibrator.getEstimatedBiasFy());
1500 assertNull(calibrator.getEstimatedBiasFz());
1501 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1502 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1503 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1504 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1505 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1506 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1507 assertNull(calibrator.getEstimatedMa());
1508 assertNull(calibrator.getEstimatedSx());
1509 assertNull(calibrator.getEstimatedSy());
1510 assertNull(calibrator.getEstimatedSz());
1511 assertNull(calibrator.getEstimatedMxy());
1512 assertNull(calibrator.getEstimatedMxz());
1513 assertNull(calibrator.getEstimatedMyx());
1514 assertNull(calibrator.getEstimatedMyz());
1515 assertNull(calibrator.getEstimatedMzx());
1516 assertNull(calibrator.getEstimatedMzy());
1517 assertNull(calibrator.getEstimatedCovariance());
1518 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1519 }
1520
1521 @Test
1522 public void testConstructor16() throws WrongSizeException {
1523 final Collection<StandardDeviationBodyKinematics> measurements =
1524 Collections.emptyList();
1525
1526 final Matrix ba = generateBa();
1527 final double biasX = ba.getElementAtIndex(0);
1528 final double biasY = ba.getElementAtIndex(1);
1529 final double biasZ = ba.getElementAtIndex(2);
1530
1531 final KnownPositionAccelerometerCalibrator calibrator =
1532 new KnownPositionAccelerometerCalibrator(measurements,
1533 true, biasX, biasY, biasZ, this);
1534
1535
1536 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1537 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1538 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1539 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1540 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1541 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1542 final Acceleration bx2 = new Acceleration(0.0,
1543 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1544 calibrator.getInitialBiasXAsAcceleration(bx2);
1545 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1546 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1547 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1548 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1549 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1550 final Acceleration by2 = new Acceleration(0.0,
1551 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1552 calibrator.getInitialBiasYAsAcceleration(by2);
1553 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1554 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1555 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1556 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1557 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1558 final Acceleration bz2 = new Acceleration(0.0,
1559 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1560 calibrator.getInitialBiasZAsAcceleration(bz2);
1561 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1562 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1563 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1564 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1565 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1566 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1567 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1568 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1569 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1570 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1571 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1572 final double[] bias1 = calibrator.getInitialBias();
1573 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
1574 final double[] bias2 = new double[3];
1575 calibrator.getInitialBias(bias2);
1576 assertArrayEquals(bias1, bias2, 0.0);
1577 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1578 assertEquals(b1, ba);
1579 final Matrix b2 = new Matrix(3, 1);
1580 calibrator.getInitialBiasAsMatrix(b2);
1581 assertEquals(b1, b2);
1582 final Matrix ma1 = calibrator.getInitialMa();
1583 assertEquals(ma1, new Matrix(3, 3));
1584 final Matrix ma2 = new Matrix(3, 3);
1585 calibrator.getInitialMa(ma2);
1586 assertEquals(ma1, ma2);
1587 assertNull(calibrator.getEcefPosition());
1588 assertNull(calibrator.getNedPosition());
1589 assertFalse(calibrator.getNedPosition(null));
1590 assertSame(calibrator.getMeasurements(), measurements);
1591 assertTrue(calibrator.isCommonAxisUsed());
1592 assertSame(calibrator.getListener(), this);
1593 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1594 assertFalse(calibrator.isReady());
1595 assertFalse(calibrator.isRunning());
1596 assertNull(calibrator.getEstimatedBiases());
1597 assertFalse(calibrator.getEstimatedBiases(null));
1598 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1599 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1600 assertNull(calibrator.getEstimatedBiasFx());
1601 assertNull(calibrator.getEstimatedBiasFy());
1602 assertNull(calibrator.getEstimatedBiasFz());
1603 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1604 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1605 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1606 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1607 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1608 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1609 assertNull(calibrator.getEstimatedMa());
1610 assertNull(calibrator.getEstimatedSx());
1611 assertNull(calibrator.getEstimatedSy());
1612 assertNull(calibrator.getEstimatedSz());
1613 assertNull(calibrator.getEstimatedMxy());
1614 assertNull(calibrator.getEstimatedMxz());
1615 assertNull(calibrator.getEstimatedMyx());
1616 assertNull(calibrator.getEstimatedMyz());
1617 assertNull(calibrator.getEstimatedMzx());
1618 assertNull(calibrator.getEstimatedMzy());
1619 assertNull(calibrator.getEstimatedCovariance());
1620 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1621 }
1622
1623 @Test
1624 public void testConstructor17() throws WrongSizeException {
1625 final Matrix ba = generateBa();
1626 final double biasX = ba.getElementAtIndex(0);
1627 final double biasY = ba.getElementAtIndex(1);
1628 final double biasZ = ba.getElementAtIndex(2);
1629
1630 final Acceleration bx = new Acceleration(biasX,
1631 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1632 final Acceleration by = new Acceleration(biasY,
1633 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1634 final Acceleration bz = new Acceleration(biasZ,
1635 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1636
1637 final KnownPositionAccelerometerCalibrator calibrator =
1638 new KnownPositionAccelerometerCalibrator(bx, by, bz);
1639
1640
1641 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1642 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1643 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1644 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1645 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1646 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1647 final Acceleration bx2 = new Acceleration(0.0,
1648 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1649 calibrator.getInitialBiasXAsAcceleration(bx2);
1650 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1651 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1652 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1653 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1654 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1655 final Acceleration by2 = new Acceleration(0.0,
1656 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1657 calibrator.getInitialBiasYAsAcceleration(by2);
1658 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1659 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1660 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1661 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1662 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1663 final Acceleration bz2 = new Acceleration(0.0,
1664 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1665 calibrator.getInitialBiasZAsAcceleration(bz2);
1666 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1667 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1668 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1669 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1670 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1671 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1672 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1673 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1674 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1675 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1676 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1677 final double[] bias1 = calibrator.getInitialBias();
1678 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
1679 final double[] bias2 = new double[3];
1680 calibrator.getInitialBias(bias2);
1681 assertArrayEquals(bias1, bias2, 0.0);
1682 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1683 assertEquals(b1, ba);
1684 final Matrix b2 = new Matrix(3, 1);
1685 calibrator.getInitialBiasAsMatrix(b2);
1686 assertEquals(b1, b2);
1687 final Matrix ma1 = calibrator.getInitialMa();
1688 assertEquals(ma1, new Matrix(3, 3));
1689 final Matrix ma2 = new Matrix(3, 3);
1690 calibrator.getInitialMa(ma2);
1691 assertEquals(ma1, ma2);
1692 assertNull(calibrator.getEcefPosition());
1693 assertNull(calibrator.getNedPosition());
1694 assertFalse(calibrator.getNedPosition(null));
1695 assertNull(calibrator.getMeasurements());
1696 assertFalse(calibrator.isCommonAxisUsed());
1697 assertNull(calibrator.getListener());
1698 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1699 assertFalse(calibrator.isReady());
1700 assertFalse(calibrator.isRunning());
1701 assertNull(calibrator.getEstimatedBiases());
1702 assertFalse(calibrator.getEstimatedBiases(null));
1703 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1704 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1705 assertNull(calibrator.getEstimatedBiasFx());
1706 assertNull(calibrator.getEstimatedBiasFy());
1707 assertNull(calibrator.getEstimatedBiasFz());
1708 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1709 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1710 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1711 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1712 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1713 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1714 assertNull(calibrator.getEstimatedMa());
1715 assertNull(calibrator.getEstimatedSx());
1716 assertNull(calibrator.getEstimatedSy());
1717 assertNull(calibrator.getEstimatedSz());
1718 assertNull(calibrator.getEstimatedMxy());
1719 assertNull(calibrator.getEstimatedMxz());
1720 assertNull(calibrator.getEstimatedMyx());
1721 assertNull(calibrator.getEstimatedMyz());
1722 assertNull(calibrator.getEstimatedMzx());
1723 assertNull(calibrator.getEstimatedMzy());
1724 assertNull(calibrator.getEstimatedCovariance());
1725 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1726 }
1727
1728 @Test
1729 public void testConstructor18() throws WrongSizeException {
1730 final Matrix ba = generateBa();
1731 final double biasX = ba.getElementAtIndex(0);
1732 final double biasY = ba.getElementAtIndex(1);
1733 final double biasZ = ba.getElementAtIndex(2);
1734
1735 final Acceleration bx = new Acceleration(biasX,
1736 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1737 final Acceleration by = new Acceleration(biasY,
1738 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1739 final Acceleration bz = new Acceleration(biasZ,
1740 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1741
1742 final KnownPositionAccelerometerCalibrator calibrator =
1743 new KnownPositionAccelerometerCalibrator(bx, by, bz,
1744 this);
1745
1746
1747 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1748 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1749 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1750 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1751 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1752 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1753 final Acceleration bx2 = new Acceleration(0.0,
1754 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1755 calibrator.getInitialBiasXAsAcceleration(bx2);
1756 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1757 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1758 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1759 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1760 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1761 final Acceleration by2 = new Acceleration(0.0,
1762 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1763 calibrator.getInitialBiasYAsAcceleration(by2);
1764 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1765 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1766 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1767 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1768 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1769 final Acceleration bz2 = new Acceleration(0.0,
1770 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1771 calibrator.getInitialBiasZAsAcceleration(bz2);
1772 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1773 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1774 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1775 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1776 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1777 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1778 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1779 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1780 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1781 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1782 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1783 final double[] bias1 = calibrator.getInitialBias();
1784 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
1785 final double[] bias2 = new double[3];
1786 calibrator.getInitialBias(bias2);
1787 assertArrayEquals(bias1, bias2, 0.0);
1788 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1789 assertEquals(b1, ba);
1790 final Matrix b2 = new Matrix(3, 1);
1791 calibrator.getInitialBiasAsMatrix(b2);
1792 assertEquals(b1, b2);
1793 final Matrix ma1 = calibrator.getInitialMa();
1794 assertEquals(ma1, new Matrix(3, 3));
1795 final Matrix ma2 = new Matrix(3, 3);
1796 calibrator.getInitialMa(ma2);
1797 assertEquals(ma1, ma2);
1798 assertNull(calibrator.getEcefPosition());
1799 assertNull(calibrator.getNedPosition());
1800 assertFalse(calibrator.getNedPosition(null));
1801 assertNull(calibrator.getMeasurements());
1802 assertFalse(calibrator.isCommonAxisUsed());
1803 assertSame(calibrator.getListener(), this);
1804 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1805 assertFalse(calibrator.isReady());
1806 assertFalse(calibrator.isRunning());
1807 assertNull(calibrator.getEstimatedBiases());
1808 assertFalse(calibrator.getEstimatedBiases(null));
1809 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1810 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1811 assertNull(calibrator.getEstimatedBiasFx());
1812 assertNull(calibrator.getEstimatedBiasFy());
1813 assertNull(calibrator.getEstimatedBiasFz());
1814 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1815 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1816 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1817 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1818 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1819 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1820 assertNull(calibrator.getEstimatedMa());
1821 assertNull(calibrator.getEstimatedSx());
1822 assertNull(calibrator.getEstimatedSy());
1823 assertNull(calibrator.getEstimatedSz());
1824 assertNull(calibrator.getEstimatedMxy());
1825 assertNull(calibrator.getEstimatedMxz());
1826 assertNull(calibrator.getEstimatedMyx());
1827 assertNull(calibrator.getEstimatedMyz());
1828 assertNull(calibrator.getEstimatedMzx());
1829 assertNull(calibrator.getEstimatedMzy());
1830 assertNull(calibrator.getEstimatedCovariance());
1831 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1832 }
1833
1834 @Test
1835 public void testConstructor19() throws WrongSizeException {
1836 final Collection<StandardDeviationBodyKinematics> measurements =
1837 Collections.emptyList();
1838
1839 final Matrix ba = generateBa();
1840 final double biasX = ba.getElementAtIndex(0);
1841 final double biasY = ba.getElementAtIndex(1);
1842 final double biasZ = ba.getElementAtIndex(2);
1843
1844 final Acceleration bx = new Acceleration(biasX,
1845 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1846 final Acceleration by = new Acceleration(biasY,
1847 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1848 final Acceleration bz = new Acceleration(biasZ,
1849 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1850
1851 final KnownPositionAccelerometerCalibrator calibrator =
1852 new KnownPositionAccelerometerCalibrator(measurements,
1853 bx, by, bz);
1854
1855
1856 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1857 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1858 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1859 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1860 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1861 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1862 final Acceleration bx2 = new Acceleration(0.0,
1863 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1864 calibrator.getInitialBiasXAsAcceleration(bx2);
1865 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1866 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1867 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1868 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1869 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1870 final Acceleration by2 = new Acceleration(0.0,
1871 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1872 calibrator.getInitialBiasYAsAcceleration(by2);
1873 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1874 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1875 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1876 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1877 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1878 final Acceleration bz2 = new Acceleration(0.0,
1879 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1880 calibrator.getInitialBiasZAsAcceleration(bz2);
1881 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1882 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1883 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1884 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1885 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1886 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1887 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1888 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1889 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1890 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1891 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1892 final double[] bias1 = calibrator.getInitialBias();
1893 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
1894 final double[] bias2 = new double[3];
1895 calibrator.getInitialBias(bias2);
1896 assertArrayEquals(bias1, bias2, 0.0);
1897 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1898 assertEquals(b1, ba);
1899 final Matrix b2 = new Matrix(3, 1);
1900 calibrator.getInitialBiasAsMatrix(b2);
1901 assertEquals(b1, b2);
1902 final Matrix ma1 = calibrator.getInitialMa();
1903 assertEquals(ma1, new Matrix(3, 3));
1904 final Matrix ma2 = new Matrix(3, 3);
1905 calibrator.getInitialMa(ma2);
1906 assertEquals(ma1, ma2);
1907 assertNull(calibrator.getEcefPosition());
1908 assertNull(calibrator.getNedPosition());
1909 assertFalse(calibrator.getNedPosition(null));
1910 assertSame(calibrator.getMeasurements(), measurements);
1911 assertFalse(calibrator.isCommonAxisUsed());
1912 assertNull(calibrator.getListener());
1913 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1914 assertFalse(calibrator.isReady());
1915 assertFalse(calibrator.isRunning());
1916 assertNull(calibrator.getEstimatedBiases());
1917 assertFalse(calibrator.getEstimatedBiases(null));
1918 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1919 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1920 assertNull(calibrator.getEstimatedBiasFx());
1921 assertNull(calibrator.getEstimatedBiasFy());
1922 assertNull(calibrator.getEstimatedBiasFz());
1923 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1924 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1925 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1926 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1927 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1928 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1929 assertNull(calibrator.getEstimatedMa());
1930 assertNull(calibrator.getEstimatedSx());
1931 assertNull(calibrator.getEstimatedSy());
1932 assertNull(calibrator.getEstimatedSz());
1933 assertNull(calibrator.getEstimatedMxy());
1934 assertNull(calibrator.getEstimatedMxz());
1935 assertNull(calibrator.getEstimatedMyx());
1936 assertNull(calibrator.getEstimatedMyz());
1937 assertNull(calibrator.getEstimatedMzx());
1938 assertNull(calibrator.getEstimatedMzy());
1939 assertNull(calibrator.getEstimatedCovariance());
1940 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1941 }
1942
1943 @Test
1944 public void testConstructor20() throws WrongSizeException {
1945 final Collection<StandardDeviationBodyKinematics> measurements =
1946 Collections.emptyList();
1947
1948 final Matrix ba = generateBa();
1949 final double biasX = ba.getElementAtIndex(0);
1950 final double biasY = ba.getElementAtIndex(1);
1951 final double biasZ = ba.getElementAtIndex(2);
1952
1953 final Acceleration bx = new Acceleration(biasX,
1954 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1955 final Acceleration by = new Acceleration(biasY,
1956 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1957 final Acceleration bz = new Acceleration(biasZ,
1958 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1959
1960 final KnownPositionAccelerometerCalibrator calibrator =
1961 new KnownPositionAccelerometerCalibrator(measurements,
1962 bx, by, bz, this);
1963
1964
1965 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1966 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1967 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1968 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1969 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1970 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1971 final Acceleration bx2 = new Acceleration(0.0,
1972 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1973 calibrator.getInitialBiasXAsAcceleration(bx2);
1974 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1975 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1976 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1977 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1978 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1979 final Acceleration by2 = new Acceleration(0.0,
1980 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1981 calibrator.getInitialBiasYAsAcceleration(by2);
1982 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1983 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1984 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1985 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1986 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1987 final Acceleration bz2 = new Acceleration(0.0,
1988 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1989 calibrator.getInitialBiasZAsAcceleration(bz2);
1990 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1991 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1992 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1993 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1994 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1995 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1996 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1997 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1998 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1999 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2000 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2001 final double[] bias1 = calibrator.getInitialBias();
2002 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
2003 final double[] bias2 = new double[3];
2004 calibrator.getInitialBias(bias2);
2005 assertArrayEquals(bias1, bias2, 0.0);
2006 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2007 assertEquals(b1, ba);
2008 final Matrix b2 = new Matrix(3, 1);
2009 calibrator.getInitialBiasAsMatrix(b2);
2010 assertEquals(b1, b2);
2011 final Matrix ma1 = calibrator.getInitialMa();
2012 assertEquals(ma1, new Matrix(3, 3));
2013 final Matrix ma2 = new Matrix(3, 3);
2014 calibrator.getInitialMa(ma2);
2015 assertEquals(ma1, ma2);
2016 assertNull(calibrator.getEcefPosition());
2017 assertNull(calibrator.getNedPosition());
2018 assertFalse(calibrator.getNedPosition(null));
2019 assertSame(calibrator.getMeasurements(), measurements);
2020 assertFalse(calibrator.isCommonAxisUsed());
2021 assertSame(calibrator.getListener(), this);
2022 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2023 assertFalse(calibrator.isReady());
2024 assertFalse(calibrator.isRunning());
2025 assertNull(calibrator.getEstimatedBiases());
2026 assertFalse(calibrator.getEstimatedBiases(null));
2027 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2028 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2029 assertNull(calibrator.getEstimatedBiasFx());
2030 assertNull(calibrator.getEstimatedBiasFy());
2031 assertNull(calibrator.getEstimatedBiasFz());
2032 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2033 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2034 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2035 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2036 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2037 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2038 assertNull(calibrator.getEstimatedMa());
2039 assertNull(calibrator.getEstimatedSx());
2040 assertNull(calibrator.getEstimatedSy());
2041 assertNull(calibrator.getEstimatedSz());
2042 assertNull(calibrator.getEstimatedMxy());
2043 assertNull(calibrator.getEstimatedMxz());
2044 assertNull(calibrator.getEstimatedMyx());
2045 assertNull(calibrator.getEstimatedMyz());
2046 assertNull(calibrator.getEstimatedMzx());
2047 assertNull(calibrator.getEstimatedMzy());
2048 assertNull(calibrator.getEstimatedCovariance());
2049 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2050 }
2051
2052 @Test
2053 public void testConstructor21() throws WrongSizeException {
2054 final Matrix ba = generateBa();
2055 final double biasX = ba.getElementAtIndex(0);
2056 final double biasY = ba.getElementAtIndex(1);
2057 final double biasZ = ba.getElementAtIndex(2);
2058
2059 final Acceleration bx = new Acceleration(biasX,
2060 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2061 final Acceleration by = new Acceleration(biasY,
2062 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2063 final Acceleration bz = new Acceleration(biasZ,
2064 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2065
2066 final KnownPositionAccelerometerCalibrator calibrator =
2067 new KnownPositionAccelerometerCalibrator(true,
2068 bx, by, bz);
2069
2070
2071 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2072 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2073 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2074 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2075 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2076 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2077 final Acceleration bx2 = new Acceleration(0.0,
2078 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2079 calibrator.getInitialBiasXAsAcceleration(bx2);
2080 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2081 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2082 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2083 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2084 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2085 final Acceleration by2 = new Acceleration(0.0,
2086 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2087 calibrator.getInitialBiasYAsAcceleration(by2);
2088 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2089 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2090 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2091 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2092 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2093 final Acceleration bz2 = new Acceleration(0.0,
2094 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2095 calibrator.getInitialBiasZAsAcceleration(bz2);
2096 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2097 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2098 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2099 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2100 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2101 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2102 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2103 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2104 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2105 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2106 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2107 final double[] bias1 = calibrator.getInitialBias();
2108 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
2109 final double[] bias2 = new double[3];
2110 calibrator.getInitialBias(bias2);
2111 assertArrayEquals(bias1, bias2, 0.0);
2112 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2113 assertEquals(b1, ba);
2114 final Matrix b2 = new Matrix(3, 1);
2115 calibrator.getInitialBiasAsMatrix(b2);
2116 assertEquals(b1, b2);
2117 final Matrix ma1 = calibrator.getInitialMa();
2118 assertEquals(ma1, new Matrix(3, 3));
2119 final Matrix ma2 = new Matrix(3, 3);
2120 calibrator.getInitialMa(ma2);
2121 assertEquals(ma1, ma2);
2122 assertNull(calibrator.getEcefPosition());
2123 assertNull(calibrator.getNedPosition());
2124 assertFalse(calibrator.getNedPosition(null));
2125 assertNull(calibrator.getMeasurements());
2126 assertTrue(calibrator.isCommonAxisUsed());
2127 assertNull(calibrator.getListener());
2128 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2129 assertFalse(calibrator.isReady());
2130 assertFalse(calibrator.isRunning());
2131 assertNull(calibrator.getEstimatedBiases());
2132 assertFalse(calibrator.getEstimatedBiases(null));
2133 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2134 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2135 assertNull(calibrator.getEstimatedBiasFx());
2136 assertNull(calibrator.getEstimatedBiasFy());
2137 assertNull(calibrator.getEstimatedBiasFz());
2138 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2139 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2140 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2141 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2142 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2143 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2144 assertNull(calibrator.getEstimatedMa());
2145 assertNull(calibrator.getEstimatedSx());
2146 assertNull(calibrator.getEstimatedSy());
2147 assertNull(calibrator.getEstimatedSz());
2148 assertNull(calibrator.getEstimatedMxy());
2149 assertNull(calibrator.getEstimatedMxz());
2150 assertNull(calibrator.getEstimatedMyx());
2151 assertNull(calibrator.getEstimatedMyz());
2152 assertNull(calibrator.getEstimatedMzx());
2153 assertNull(calibrator.getEstimatedMzy());
2154 assertNull(calibrator.getEstimatedCovariance());
2155 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2156 }
2157
2158 @Test
2159 public void testConstructor22() throws WrongSizeException {
2160 final Matrix ba = generateBa();
2161 final double biasX = ba.getElementAtIndex(0);
2162 final double biasY = ba.getElementAtIndex(1);
2163 final double biasZ = ba.getElementAtIndex(2);
2164
2165 final Acceleration bx = new Acceleration(biasX,
2166 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2167 final Acceleration by = new Acceleration(biasY,
2168 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2169 final Acceleration bz = new Acceleration(biasZ,
2170 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2171
2172 final KnownPositionAccelerometerCalibrator calibrator =
2173 new KnownPositionAccelerometerCalibrator(true,
2174 bx, by, bz, this);
2175
2176
2177 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2178 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2179 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2180 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2181 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2182 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2183 final Acceleration bx2 = new Acceleration(0.0,
2184 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2185 calibrator.getInitialBiasXAsAcceleration(bx2);
2186 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2187 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2188 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2189 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2190 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2191 final Acceleration by2 = new Acceleration(0.0,
2192 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2193 calibrator.getInitialBiasYAsAcceleration(by2);
2194 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2195 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2196 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2197 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2198 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2199 final Acceleration bz2 = new Acceleration(0.0,
2200 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2201 calibrator.getInitialBiasZAsAcceleration(bz2);
2202 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2203 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2204 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2205 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2206 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2207 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2208 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2209 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2210 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2211 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2212 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2213 final double[] bias1 = calibrator.getInitialBias();
2214 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
2215 final double[] bias2 = new double[3];
2216 calibrator.getInitialBias(bias2);
2217 assertArrayEquals(bias1, bias2, 0.0);
2218 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2219 assertEquals(b1, ba);
2220 final Matrix b2 = new Matrix(3, 1);
2221 calibrator.getInitialBiasAsMatrix(b2);
2222 assertEquals(b1, b2);
2223 final Matrix ma1 = calibrator.getInitialMa();
2224 assertEquals(ma1, new Matrix(3, 3));
2225 final Matrix ma2 = new Matrix(3, 3);
2226 calibrator.getInitialMa(ma2);
2227 assertEquals(ma1, ma2);
2228 assertNull(calibrator.getEcefPosition());
2229 assertNull(calibrator.getNedPosition());
2230 assertFalse(calibrator.getNedPosition(null));
2231 assertNull(calibrator.getMeasurements());
2232 assertTrue(calibrator.isCommonAxisUsed());
2233 assertSame(calibrator.getListener(), this);
2234 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2235 assertFalse(calibrator.isReady());
2236 assertFalse(calibrator.isRunning());
2237 assertNull(calibrator.getEstimatedBiases());
2238 assertFalse(calibrator.getEstimatedBiases(null));
2239 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2240 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2241 assertNull(calibrator.getEstimatedBiasFx());
2242 assertNull(calibrator.getEstimatedBiasFy());
2243 assertNull(calibrator.getEstimatedBiasFz());
2244 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2245 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2246 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2247 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2248 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2249 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2250 assertNull(calibrator.getEstimatedMa());
2251 assertNull(calibrator.getEstimatedSx());
2252 assertNull(calibrator.getEstimatedSy());
2253 assertNull(calibrator.getEstimatedSz());
2254 assertNull(calibrator.getEstimatedMxy());
2255 assertNull(calibrator.getEstimatedMxz());
2256 assertNull(calibrator.getEstimatedMyx());
2257 assertNull(calibrator.getEstimatedMyz());
2258 assertNull(calibrator.getEstimatedMzx());
2259 assertNull(calibrator.getEstimatedMzy());
2260 assertNull(calibrator.getEstimatedCovariance());
2261 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2262 }
2263
2264 @Test
2265 public void testConstructor23() throws WrongSizeException {
2266 final Collection<StandardDeviationBodyKinematics> measurements =
2267 Collections.emptyList();
2268
2269 final Matrix ba = generateBa();
2270 final double biasX = ba.getElementAtIndex(0);
2271 final double biasY = ba.getElementAtIndex(1);
2272 final double biasZ = ba.getElementAtIndex(2);
2273
2274 final Acceleration bx = new Acceleration(biasX,
2275 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2276 final Acceleration by = new Acceleration(biasY,
2277 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2278 final Acceleration bz = new Acceleration(biasZ,
2279 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2280
2281 final KnownPositionAccelerometerCalibrator calibrator =
2282 new KnownPositionAccelerometerCalibrator(measurements,
2283 true, bx, by, bz);
2284
2285
2286 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2287 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2288 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2289 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2290 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2291 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2292 final Acceleration bx2 = new Acceleration(0.0,
2293 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2294 calibrator.getInitialBiasXAsAcceleration(bx2);
2295 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2296 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2297 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2298 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2299 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2300 final Acceleration by2 = new Acceleration(0.0,
2301 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2302 calibrator.getInitialBiasYAsAcceleration(by2);
2303 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2304 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2305 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2306 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2307 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2308 final Acceleration bz2 = new Acceleration(0.0,
2309 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2310 calibrator.getInitialBiasZAsAcceleration(bz2);
2311 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2312 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2313 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2314 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2315 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2316 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2317 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2318 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2319 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2320 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2321 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2322 final double[] bias1 = calibrator.getInitialBias();
2323 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
2324 final double[] bias2 = new double[3];
2325 calibrator.getInitialBias(bias2);
2326 assertArrayEquals(bias1, bias2, 0.0);
2327 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2328 assertEquals(b1, ba);
2329 final Matrix b2 = new Matrix(3, 1);
2330 calibrator.getInitialBiasAsMatrix(b2);
2331 assertEquals(b1, b2);
2332 final Matrix ma1 = calibrator.getInitialMa();
2333 assertEquals(ma1, new Matrix(3, 3));
2334 final Matrix ma2 = new Matrix(3, 3);
2335 calibrator.getInitialMa(ma2);
2336 assertEquals(ma1, ma2);
2337 assertNull(calibrator.getEcefPosition());
2338 assertNull(calibrator.getNedPosition());
2339 assertFalse(calibrator.getNedPosition(null));
2340 assertSame(calibrator.getMeasurements(), measurements);
2341 assertTrue(calibrator.isCommonAxisUsed());
2342 assertNull(calibrator.getListener());
2343 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2344 assertFalse(calibrator.isReady());
2345 assertFalse(calibrator.isRunning());
2346 assertNull(calibrator.getEstimatedBiases());
2347 assertFalse(calibrator.getEstimatedBiases(null));
2348 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2349 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2350 assertNull(calibrator.getEstimatedBiasFx());
2351 assertNull(calibrator.getEstimatedBiasFy());
2352 assertNull(calibrator.getEstimatedBiasFz());
2353 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2354 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2355 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2356 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2357 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2358 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2359 assertNull(calibrator.getEstimatedMa());
2360 assertNull(calibrator.getEstimatedSx());
2361 assertNull(calibrator.getEstimatedSy());
2362 assertNull(calibrator.getEstimatedSz());
2363 assertNull(calibrator.getEstimatedMxy());
2364 assertNull(calibrator.getEstimatedMxz());
2365 assertNull(calibrator.getEstimatedMyx());
2366 assertNull(calibrator.getEstimatedMyz());
2367 assertNull(calibrator.getEstimatedMzx());
2368 assertNull(calibrator.getEstimatedMzy());
2369 assertNull(calibrator.getEstimatedCovariance());
2370 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2371 }
2372
2373 @Test
2374 public void testConstructor24() throws WrongSizeException {
2375 final Collection<StandardDeviationBodyKinematics> measurements =
2376 Collections.emptyList();
2377
2378 final Matrix ba = generateBa();
2379 final double biasX = ba.getElementAtIndex(0);
2380 final double biasY = ba.getElementAtIndex(1);
2381 final double biasZ = ba.getElementAtIndex(2);
2382
2383 final Acceleration bx = new Acceleration(biasX,
2384 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2385 final Acceleration by = new Acceleration(biasY,
2386 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2387 final Acceleration bz = new Acceleration(biasZ,
2388 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2389
2390 final KnownPositionAccelerometerCalibrator calibrator =
2391 new KnownPositionAccelerometerCalibrator(measurements,
2392 true, bx, by, bz, this);
2393
2394
2395 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2396 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2397 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2398 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2399 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2400 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2401 final Acceleration bx2 = new Acceleration(0.0,
2402 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2403 calibrator.getInitialBiasXAsAcceleration(bx2);
2404 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2405 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2406 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2407 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2408 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2409 final Acceleration by2 = new Acceleration(0.0,
2410 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2411 calibrator.getInitialBiasYAsAcceleration(by2);
2412 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2413 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2414 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2415 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2416 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2417 final Acceleration bz2 = new Acceleration(0.0,
2418 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2419 calibrator.getInitialBiasZAsAcceleration(bz2);
2420 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2421 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2422 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2423 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2424 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2425 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2426 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2427 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2428 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2429 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2430 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2431 final double[] bias1 = calibrator.getInitialBias();
2432 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
2433 final double[] bias2 = new double[3];
2434 calibrator.getInitialBias(bias2);
2435 assertArrayEquals(bias1, bias2, 0.0);
2436 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2437 assertEquals(b1, ba);
2438 final Matrix b2 = new Matrix(3, 1);
2439 calibrator.getInitialBiasAsMatrix(b2);
2440 assertEquals(b1, b2);
2441 final Matrix ma1 = calibrator.getInitialMa();
2442 assertEquals(ma1, new Matrix(3, 3));
2443 final Matrix ma2 = new Matrix(3, 3);
2444 calibrator.getInitialMa(ma2);
2445 assertEquals(ma1, ma2);
2446 assertNull(calibrator.getEcefPosition());
2447 assertNull(calibrator.getNedPosition());
2448 assertFalse(calibrator.getNedPosition(null));
2449 assertSame(calibrator.getMeasurements(), measurements);
2450 assertTrue(calibrator.isCommonAxisUsed());
2451 assertSame(calibrator.getListener(), this);
2452 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2453 assertFalse(calibrator.isReady());
2454 assertFalse(calibrator.isRunning());
2455 assertNull(calibrator.getEstimatedBiases());
2456 assertFalse(calibrator.getEstimatedBiases(null));
2457 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2458 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2459 assertNull(calibrator.getEstimatedBiasFx());
2460 assertNull(calibrator.getEstimatedBiasFy());
2461 assertNull(calibrator.getEstimatedBiasFz());
2462 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2463 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2464 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2465 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2466 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2467 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2468 assertNull(calibrator.getEstimatedMa());
2469 assertNull(calibrator.getEstimatedSx());
2470 assertNull(calibrator.getEstimatedSy());
2471 assertNull(calibrator.getEstimatedSz());
2472 assertNull(calibrator.getEstimatedMxy());
2473 assertNull(calibrator.getEstimatedMxz());
2474 assertNull(calibrator.getEstimatedMyx());
2475 assertNull(calibrator.getEstimatedMyz());
2476 assertNull(calibrator.getEstimatedMzx());
2477 assertNull(calibrator.getEstimatedMzy());
2478 assertNull(calibrator.getEstimatedCovariance());
2479 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2480 }
2481
2482 @Test
2483 public void testConstructor25() throws WrongSizeException {
2484 final Matrix ba = generateBa();
2485 final double biasX = ba.getElementAtIndex(0);
2486 final double biasY = ba.getElementAtIndex(1);
2487 final double biasZ = ba.getElementAtIndex(2);
2488
2489 final Matrix ma = generateMaCommonAxis();
2490 final double sx = ma.getElementAt(0, 0);
2491 final double sy = ma.getElementAt(1, 1);
2492 final double sz = ma.getElementAt(2, 2);
2493
2494 final KnownPositionAccelerometerCalibrator calibrator =
2495 new KnownPositionAccelerometerCalibrator(biasX, biasY, biasZ,
2496 sx, sy, sz);
2497
2498
2499 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2500 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2501 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2502 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2503 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2504 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2505 final Acceleration bx2 = new Acceleration(0.0,
2506 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2507 calibrator.getInitialBiasXAsAcceleration(bx2);
2508 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2509 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2510 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2511 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2512 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2513 final Acceleration by2 = new Acceleration(0.0,
2514 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2515 calibrator.getInitialBiasYAsAcceleration(by2);
2516 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2517 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2518 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2519 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2520 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2521 final Acceleration bz2 = new Acceleration(0.0,
2522 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2523 calibrator.getInitialBiasZAsAcceleration(bz2);
2524 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2525 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2526 assertEquals(calibrator.getInitialSx(), sx, 0.0);
2527 assertEquals(calibrator.getInitialSy(), sy, 0.0);
2528 assertEquals(calibrator.getInitialSz(), sz, 0.0);
2529 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2530 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2531 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2532 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2533 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2534 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2535 final double[] bias1 = calibrator.getInitialBias();
2536 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
2537 final double[] bias2 = new double[3];
2538 calibrator.getInitialBias(bias2);
2539 assertArrayEquals(bias1, bias2, 0.0);
2540 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2541 assertEquals(b1, ba);
2542 final Matrix b2 = new Matrix(3, 1);
2543 calibrator.getInitialBiasAsMatrix(b2);
2544 assertEquals(b1, b2);
2545 final Matrix ma1 = calibrator.getInitialMa();
2546 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
2547 final Matrix ma2 = new Matrix(3, 3);
2548 calibrator.getInitialMa(ma2);
2549 assertEquals(ma1, ma2);
2550 assertNull(calibrator.getEcefPosition());
2551 assertNull(calibrator.getNedPosition());
2552 assertFalse(calibrator.getNedPosition(null));
2553 assertNull(calibrator.getMeasurements());
2554 assertFalse(calibrator.isCommonAxisUsed());
2555 assertNull(calibrator.getListener());
2556 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2557 assertFalse(calibrator.isReady());
2558 assertFalse(calibrator.isRunning());
2559 assertNull(calibrator.getEstimatedBiases());
2560 assertFalse(calibrator.getEstimatedBiases(null));
2561 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2562 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2563 assertNull(calibrator.getEstimatedBiasFx());
2564 assertNull(calibrator.getEstimatedBiasFy());
2565 assertNull(calibrator.getEstimatedBiasFz());
2566 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2567 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2568 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2569 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2570 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2571 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2572 assertNull(calibrator.getEstimatedMa());
2573 assertNull(calibrator.getEstimatedSx());
2574 assertNull(calibrator.getEstimatedSy());
2575 assertNull(calibrator.getEstimatedSz());
2576 assertNull(calibrator.getEstimatedMxy());
2577 assertNull(calibrator.getEstimatedMxz());
2578 assertNull(calibrator.getEstimatedMyx());
2579 assertNull(calibrator.getEstimatedMyz());
2580 assertNull(calibrator.getEstimatedMzx());
2581 assertNull(calibrator.getEstimatedMzy());
2582 assertNull(calibrator.getEstimatedCovariance());
2583 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2584 }
2585
2586 @Test
2587 public void testConstructor26() throws WrongSizeException {
2588 final Collection<StandardDeviationBodyKinematics> measurements =
2589 Collections.emptyList();
2590
2591 final Matrix ba = generateBa();
2592 final double biasX = ba.getElementAtIndex(0);
2593 final double biasY = ba.getElementAtIndex(1);
2594 final double biasZ = ba.getElementAtIndex(2);
2595
2596 final Matrix ma = generateMaCommonAxis();
2597 final double sx = ma.getElementAt(0, 0);
2598 final double sy = ma.getElementAt(1, 1);
2599 final double sz = ma.getElementAt(2, 2);
2600
2601 final KnownPositionAccelerometerCalibrator calibrator =
2602 new KnownPositionAccelerometerCalibrator(measurements,
2603 biasX, biasY, biasZ, sx, sy, sz);
2604
2605
2606 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2607 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2608 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2609 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2610 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2611 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2612 final Acceleration bx2 = new Acceleration(0.0,
2613 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2614 calibrator.getInitialBiasXAsAcceleration(bx2);
2615 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2616 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2617 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2618 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2619 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2620 final Acceleration by2 = new Acceleration(0.0,
2621 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2622 calibrator.getInitialBiasYAsAcceleration(by2);
2623 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2624 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2625 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2626 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2627 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2628 final Acceleration bz2 = new Acceleration(0.0,
2629 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2630 calibrator.getInitialBiasZAsAcceleration(bz2);
2631 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2632 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2633 assertEquals(calibrator.getInitialSx(), sx, 0.0);
2634 assertEquals(calibrator.getInitialSy(), sy, 0.0);
2635 assertEquals(calibrator.getInitialSz(), sz, 0.0);
2636 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2637 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2638 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2639 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2640 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2641 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2642 final double[] bias1 = calibrator.getInitialBias();
2643 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
2644 final double[] bias2 = new double[3];
2645 calibrator.getInitialBias(bias2);
2646 assertArrayEquals(bias1, bias2, 0.0);
2647 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2648 assertEquals(b1, ba);
2649 final Matrix b2 = new Matrix(3, 1);
2650 calibrator.getInitialBiasAsMatrix(b2);
2651 assertEquals(b1, b2);
2652 final Matrix ma1 = calibrator.getInitialMa();
2653 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
2654 final Matrix ma2 = new Matrix(3, 3);
2655 calibrator.getInitialMa(ma2);
2656 assertEquals(ma1, ma2);
2657 assertNull(calibrator.getEcefPosition());
2658 assertNull(calibrator.getNedPosition());
2659 assertFalse(calibrator.getNedPosition(null));
2660 assertSame(calibrator.getMeasurements(), measurements);
2661 assertFalse(calibrator.isCommonAxisUsed());
2662 assertNull(calibrator.getListener());
2663 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2664 assertFalse(calibrator.isReady());
2665 assertFalse(calibrator.isRunning());
2666 assertNull(calibrator.getEstimatedBiases());
2667 assertFalse(calibrator.getEstimatedBiases(null));
2668 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2669 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2670 assertNull(calibrator.getEstimatedBiasFx());
2671 assertNull(calibrator.getEstimatedBiasFy());
2672 assertNull(calibrator.getEstimatedBiasFz());
2673 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2674 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2675 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2676 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2677 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2678 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2679 assertNull(calibrator.getEstimatedMa());
2680 assertNull(calibrator.getEstimatedSx());
2681 assertNull(calibrator.getEstimatedSy());
2682 assertNull(calibrator.getEstimatedSz());
2683 assertNull(calibrator.getEstimatedMxy());
2684 assertNull(calibrator.getEstimatedMxz());
2685 assertNull(calibrator.getEstimatedMyx());
2686 assertNull(calibrator.getEstimatedMyz());
2687 assertNull(calibrator.getEstimatedMzx());
2688 assertNull(calibrator.getEstimatedMzy());
2689 assertNull(calibrator.getEstimatedCovariance());
2690 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2691 }
2692
2693 @Test
2694 public void testConstructor27() throws WrongSizeException {
2695 final Collection<StandardDeviationBodyKinematics> measurements =
2696 Collections.emptyList();
2697
2698 final Matrix ba = generateBa();
2699 final double biasX = ba.getElementAtIndex(0);
2700 final double biasY = ba.getElementAtIndex(1);
2701 final double biasZ = ba.getElementAtIndex(2);
2702
2703 final Matrix ma = generateMaCommonAxis();
2704 final double sx = ma.getElementAt(0, 0);
2705 final double sy = ma.getElementAt(1, 1);
2706 final double sz = ma.getElementAt(2, 2);
2707
2708 final KnownPositionAccelerometerCalibrator calibrator =
2709 new KnownPositionAccelerometerCalibrator(measurements,
2710 biasX, biasY, biasZ, sx, sy, sz, this);
2711
2712
2713 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2714 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2715 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2716 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2717 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2718 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2719 final Acceleration bx2 = new Acceleration(0.0,
2720 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2721 calibrator.getInitialBiasXAsAcceleration(bx2);
2722 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2723 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2724 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2725 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2726 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2727 final Acceleration by2 = new Acceleration(0.0,
2728 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2729 calibrator.getInitialBiasYAsAcceleration(by2);
2730 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2731 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2732 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2733 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2734 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2735 final Acceleration bz2 = new Acceleration(0.0,
2736 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2737 calibrator.getInitialBiasZAsAcceleration(bz2);
2738 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2739 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2740 assertEquals(calibrator.getInitialSx(), sx, 0.0);
2741 assertEquals(calibrator.getInitialSy(), sy, 0.0);
2742 assertEquals(calibrator.getInitialSz(), sz, 0.0);
2743 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2744 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2745 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2746 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2747 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2748 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2749 final double[] bias1 = calibrator.getInitialBias();
2750 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
2751 final double[] bias2 = new double[3];
2752 calibrator.getInitialBias(bias2);
2753 assertArrayEquals(bias1, bias2, 0.0);
2754 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2755 assertEquals(b1, ba);
2756 final Matrix b2 = new Matrix(3, 1);
2757 calibrator.getInitialBiasAsMatrix(b2);
2758 assertEquals(b1, b2);
2759 final Matrix ma1 = calibrator.getInitialMa();
2760 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
2761 final Matrix ma2 = new Matrix(3, 3);
2762 calibrator.getInitialMa(ma2);
2763 assertEquals(ma1, ma2);
2764 assertNull(calibrator.getEcefPosition());
2765 assertNull(calibrator.getNedPosition());
2766 assertFalse(calibrator.getNedPosition(null));
2767 assertSame(calibrator.getMeasurements(), measurements);
2768 assertFalse(calibrator.isCommonAxisUsed());
2769 assertSame(calibrator.getListener(), this);
2770 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2771 assertFalse(calibrator.isReady());
2772 assertFalse(calibrator.isRunning());
2773 assertNull(calibrator.getEstimatedBiases());
2774 assertFalse(calibrator.getEstimatedBiases(null));
2775 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2776 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2777 assertNull(calibrator.getEstimatedBiasFx());
2778 assertNull(calibrator.getEstimatedBiasFy());
2779 assertNull(calibrator.getEstimatedBiasFz());
2780 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2781 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2782 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2783 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2784 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2785 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2786 assertNull(calibrator.getEstimatedMa());
2787 assertNull(calibrator.getEstimatedSx());
2788 assertNull(calibrator.getEstimatedSy());
2789 assertNull(calibrator.getEstimatedSz());
2790 assertNull(calibrator.getEstimatedMxy());
2791 assertNull(calibrator.getEstimatedMxz());
2792 assertNull(calibrator.getEstimatedMyx());
2793 assertNull(calibrator.getEstimatedMyz());
2794 assertNull(calibrator.getEstimatedMzx());
2795 assertNull(calibrator.getEstimatedMzy());
2796 assertNull(calibrator.getEstimatedCovariance());
2797 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2798 }
2799
2800 @Test
2801 public void testConstructor28() throws WrongSizeException {
2802 final Matrix ba = generateBa();
2803 final double biasX = ba.getElementAtIndex(0);
2804 final double biasY = ba.getElementAtIndex(1);
2805 final double biasZ = ba.getElementAtIndex(2);
2806
2807 final Matrix ma = generateMaCommonAxis();
2808 final double sx = ma.getElementAt(0, 0);
2809 final double sy = ma.getElementAt(1, 1);
2810 final double sz = ma.getElementAt(2, 2);
2811
2812 final KnownPositionAccelerometerCalibrator calibrator =
2813 new KnownPositionAccelerometerCalibrator(true,
2814 biasX, biasY, biasZ, sx, sy, sz);
2815
2816
2817 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2818 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2819 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2820 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2821 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2822 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2823 final Acceleration bx2 = new Acceleration(0.0,
2824 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2825 calibrator.getInitialBiasXAsAcceleration(bx2);
2826 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2827 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2828 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2829 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2830 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2831 final Acceleration by2 = new Acceleration(0.0,
2832 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2833 calibrator.getInitialBiasYAsAcceleration(by2);
2834 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2835 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2836 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2837 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2838 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2839 final Acceleration bz2 = new Acceleration(0.0,
2840 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2841 calibrator.getInitialBiasZAsAcceleration(bz2);
2842 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2843 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2844 assertEquals(calibrator.getInitialSx(), sx, 0.0);
2845 assertEquals(calibrator.getInitialSy(), sy, 0.0);
2846 assertEquals(calibrator.getInitialSz(), sz, 0.0);
2847 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2848 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2849 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2850 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2851 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2852 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2853 final double[] bias1 = calibrator.getInitialBias();
2854 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
2855 final double[] bias2 = new double[3];
2856 calibrator.getInitialBias(bias2);
2857 assertArrayEquals(bias1, bias2, 0.0);
2858 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2859 assertEquals(b1, ba);
2860 final Matrix b2 = new Matrix(3, 1);
2861 calibrator.getInitialBiasAsMatrix(b2);
2862 assertEquals(b1, b2);
2863 final Matrix ma1 = calibrator.getInitialMa();
2864 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
2865 final Matrix ma2 = new Matrix(3, 3);
2866 calibrator.getInitialMa(ma2);
2867 assertEquals(ma1, ma2);
2868 assertNull(calibrator.getEcefPosition());
2869 assertNull(calibrator.getNedPosition());
2870 assertFalse(calibrator.getNedPosition(null));
2871 assertNull(calibrator.getMeasurements());
2872 assertTrue(calibrator.isCommonAxisUsed());
2873 assertNull(calibrator.getListener());
2874 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2875 assertFalse(calibrator.isReady());
2876 assertFalse(calibrator.isRunning());
2877 assertNull(calibrator.getEstimatedBiases());
2878 assertFalse(calibrator.getEstimatedBiases(null));
2879 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2880 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2881 assertNull(calibrator.getEstimatedBiasFx());
2882 assertNull(calibrator.getEstimatedBiasFy());
2883 assertNull(calibrator.getEstimatedBiasFz());
2884 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2885 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2886 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2887 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2888 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2889 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2890 assertNull(calibrator.getEstimatedMa());
2891 assertNull(calibrator.getEstimatedSx());
2892 assertNull(calibrator.getEstimatedSy());
2893 assertNull(calibrator.getEstimatedSz());
2894 assertNull(calibrator.getEstimatedMxy());
2895 assertNull(calibrator.getEstimatedMxz());
2896 assertNull(calibrator.getEstimatedMyx());
2897 assertNull(calibrator.getEstimatedMyz());
2898 assertNull(calibrator.getEstimatedMzx());
2899 assertNull(calibrator.getEstimatedMzy());
2900 assertNull(calibrator.getEstimatedCovariance());
2901 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2902 }
2903
2904 @Test
2905 public void testConstructor29() throws WrongSizeException {
2906 final Matrix ba = generateBa();
2907 final double biasX = ba.getElementAtIndex(0);
2908 final double biasY = ba.getElementAtIndex(1);
2909 final double biasZ = ba.getElementAtIndex(2);
2910
2911 final Matrix ma = generateMaCommonAxis();
2912 final double sx = ma.getElementAt(0, 0);
2913 final double sy = ma.getElementAt(1, 1);
2914 final double sz = ma.getElementAt(2, 2);
2915
2916 final KnownPositionAccelerometerCalibrator calibrator =
2917 new KnownPositionAccelerometerCalibrator(true,
2918 biasX, biasY, biasZ, sx, sy, sz, this);
2919
2920
2921 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2922 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2923 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2924 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2925 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2926 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2927 final Acceleration bx2 = new Acceleration(0.0,
2928 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2929 calibrator.getInitialBiasXAsAcceleration(bx2);
2930 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2931 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2932 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2933 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2934 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2935 final Acceleration by2 = new Acceleration(0.0,
2936 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2937 calibrator.getInitialBiasYAsAcceleration(by2);
2938 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2939 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2940 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2941 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2942 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2943 final Acceleration bz2 = new Acceleration(0.0,
2944 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2945 calibrator.getInitialBiasZAsAcceleration(bz2);
2946 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2947 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2948 assertEquals(calibrator.getInitialSx(), sx, 0.0);
2949 assertEquals(calibrator.getInitialSy(), sy, 0.0);
2950 assertEquals(calibrator.getInitialSz(), sz, 0.0);
2951 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2952 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2953 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2954 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2955 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2956 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2957 final double[] bias1 = calibrator.getInitialBias();
2958 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
2959 final double[] bias2 = new double[3];
2960 calibrator.getInitialBias(bias2);
2961 assertArrayEquals(bias1, bias2, 0.0);
2962 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2963 assertEquals(b1, ba);
2964 final Matrix b2 = new Matrix(3, 1);
2965 calibrator.getInitialBiasAsMatrix(b2);
2966 assertEquals(b1, b2);
2967 final Matrix ma1 = calibrator.getInitialMa();
2968 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
2969 final Matrix ma2 = new Matrix(3, 3);
2970 calibrator.getInitialMa(ma2);
2971 assertEquals(ma1, ma2);
2972 assertNull(calibrator.getEcefPosition());
2973 assertNull(calibrator.getNedPosition());
2974 assertFalse(calibrator.getNedPosition(null));
2975 assertNull(calibrator.getMeasurements());
2976 assertTrue(calibrator.isCommonAxisUsed());
2977 assertSame(calibrator.getListener(), this);
2978 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2979 assertFalse(calibrator.isReady());
2980 assertFalse(calibrator.isRunning());
2981 assertNull(calibrator.getEstimatedBiases());
2982 assertFalse(calibrator.getEstimatedBiases(null));
2983 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2984 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2985 assertNull(calibrator.getEstimatedBiasFx());
2986 assertNull(calibrator.getEstimatedBiasFy());
2987 assertNull(calibrator.getEstimatedBiasFz());
2988 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2989 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2990 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2991 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2992 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2993 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2994 assertNull(calibrator.getEstimatedMa());
2995 assertNull(calibrator.getEstimatedSx());
2996 assertNull(calibrator.getEstimatedSy());
2997 assertNull(calibrator.getEstimatedSz());
2998 assertNull(calibrator.getEstimatedMxy());
2999 assertNull(calibrator.getEstimatedMxz());
3000 assertNull(calibrator.getEstimatedMyx());
3001 assertNull(calibrator.getEstimatedMyz());
3002 assertNull(calibrator.getEstimatedMzx());
3003 assertNull(calibrator.getEstimatedMzy());
3004 assertNull(calibrator.getEstimatedCovariance());
3005 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3006 }
3007
3008 @Test
3009 public void testConstructor30() throws WrongSizeException {
3010 final Collection<StandardDeviationBodyKinematics> measurements =
3011 Collections.emptyList();
3012 final Matrix ba = generateBa();
3013 final double biasX = ba.getElementAtIndex(0);
3014 final double biasY = ba.getElementAtIndex(1);
3015 final double biasZ = ba.getElementAtIndex(2);
3016
3017 final Matrix ma = generateMaCommonAxis();
3018 final double sx = ma.getElementAt(0, 0);
3019 final double sy = ma.getElementAt(1, 1);
3020 final double sz = ma.getElementAt(2, 2);
3021
3022 final KnownPositionAccelerometerCalibrator calibrator =
3023 new KnownPositionAccelerometerCalibrator(measurements,
3024 true, biasX, biasY, biasZ, sx, sy, sz);
3025
3026
3027 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3028 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3029 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3030 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3031 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3032 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3033 final Acceleration bx2 = new Acceleration(0.0,
3034 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3035 calibrator.getInitialBiasXAsAcceleration(bx2);
3036 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3037 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3038 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3039 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3040 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3041 final Acceleration by2 = new Acceleration(0.0,
3042 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3043 calibrator.getInitialBiasYAsAcceleration(by2);
3044 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3045 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3046 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3047 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3048 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3049 final Acceleration bz2 = new Acceleration(0.0,
3050 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3051 calibrator.getInitialBiasZAsAcceleration(bz2);
3052 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3053 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3054 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3055 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3056 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3057 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3058 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3059 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3060 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3061 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3062 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3063 final double[] bias1 = calibrator.getInitialBias();
3064 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
3065 final double[] bias2 = new double[3];
3066 calibrator.getInitialBias(bias2);
3067 assertArrayEquals(bias1, bias2, 0.0);
3068 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3069 assertEquals(b1, ba);
3070 final Matrix b2 = new Matrix(3, 1);
3071 calibrator.getInitialBiasAsMatrix(b2);
3072 assertEquals(b1, b2);
3073 final Matrix ma1 = calibrator.getInitialMa();
3074 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
3075 final Matrix ma2 = new Matrix(3, 3);
3076 calibrator.getInitialMa(ma2);
3077 assertEquals(ma1, ma2);
3078 assertNull(calibrator.getEcefPosition());
3079 assertNull(calibrator.getNedPosition());
3080 assertFalse(calibrator.getNedPosition(null));
3081 assertSame(calibrator.getMeasurements(), measurements);
3082 assertTrue(calibrator.isCommonAxisUsed());
3083 assertNull(calibrator.getListener());
3084 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3085 assertFalse(calibrator.isReady());
3086 assertFalse(calibrator.isRunning());
3087 assertNull(calibrator.getEstimatedBiases());
3088 assertFalse(calibrator.getEstimatedBiases(null));
3089 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3090 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3091 assertNull(calibrator.getEstimatedBiasFx());
3092 assertNull(calibrator.getEstimatedBiasFy());
3093 assertNull(calibrator.getEstimatedBiasFz());
3094 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3095 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3096 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3097 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3098 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3099 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3100 assertNull(calibrator.getEstimatedMa());
3101 assertNull(calibrator.getEstimatedSx());
3102 assertNull(calibrator.getEstimatedSy());
3103 assertNull(calibrator.getEstimatedSz());
3104 assertNull(calibrator.getEstimatedMxy());
3105 assertNull(calibrator.getEstimatedMxz());
3106 assertNull(calibrator.getEstimatedMyx());
3107 assertNull(calibrator.getEstimatedMyz());
3108 assertNull(calibrator.getEstimatedMzx());
3109 assertNull(calibrator.getEstimatedMzy());
3110 assertNull(calibrator.getEstimatedCovariance());
3111 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3112 }
3113
3114 @Test
3115 public void testConstructor31() throws WrongSizeException {
3116 final Collection<StandardDeviationBodyKinematics> measurements =
3117 Collections.emptyList();
3118 final Matrix ba = generateBa();
3119 final double biasX = ba.getElementAtIndex(0);
3120 final double biasY = ba.getElementAtIndex(1);
3121 final double biasZ = ba.getElementAtIndex(2);
3122
3123 final Matrix ma = generateMaCommonAxis();
3124 final double sx = ma.getElementAt(0, 0);
3125 final double sy = ma.getElementAt(1, 1);
3126 final double sz = ma.getElementAt(2, 2);
3127
3128 final KnownPositionAccelerometerCalibrator calibrator =
3129 new KnownPositionAccelerometerCalibrator(measurements,
3130 true, biasX, biasY, biasZ, sx, sy, sz,
3131 this);
3132
3133
3134 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3135 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3136 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3137 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3138 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3139 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3140 final Acceleration bx2 = new Acceleration(0.0,
3141 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3142 calibrator.getInitialBiasXAsAcceleration(bx2);
3143 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3144 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3145 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3146 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3147 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3148 final Acceleration by2 = new Acceleration(0.0,
3149 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3150 calibrator.getInitialBiasYAsAcceleration(by2);
3151 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3152 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3153 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3154 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3155 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3156 final Acceleration bz2 = new Acceleration(0.0,
3157 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3158 calibrator.getInitialBiasZAsAcceleration(bz2);
3159 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3160 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3161 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3162 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3163 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3164 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3165 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3166 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3167 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3168 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3169 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3170 final double[] bias1 = calibrator.getInitialBias();
3171 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
3172 final double[] bias2 = new double[3];
3173 calibrator.getInitialBias(bias2);
3174 assertArrayEquals(bias1, bias2, 0.0);
3175 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3176 assertEquals(b1, ba);
3177 final Matrix b2 = new Matrix(3, 1);
3178 calibrator.getInitialBiasAsMatrix(b2);
3179 assertEquals(b1, b2);
3180 final Matrix ma1 = calibrator.getInitialMa();
3181 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
3182 final Matrix ma2 = new Matrix(3, 3);
3183 calibrator.getInitialMa(ma2);
3184 assertEquals(ma1, ma2);
3185 assertNull(calibrator.getEcefPosition());
3186 assertNull(calibrator.getNedPosition());
3187 assertFalse(calibrator.getNedPosition(null));
3188 assertSame(calibrator.getMeasurements(), measurements);
3189 assertTrue(calibrator.isCommonAxisUsed());
3190 assertSame(calibrator.getListener(), this);
3191 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3192 assertFalse(calibrator.isReady());
3193 assertFalse(calibrator.isRunning());
3194 assertNull(calibrator.getEstimatedBiases());
3195 assertFalse(calibrator.getEstimatedBiases(null));
3196 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3197 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3198 assertNull(calibrator.getEstimatedBiasFx());
3199 assertNull(calibrator.getEstimatedBiasFy());
3200 assertNull(calibrator.getEstimatedBiasFz());
3201 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3202 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3203 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3204 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3205 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3206 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3207 assertNull(calibrator.getEstimatedMa());
3208 assertNull(calibrator.getEstimatedSx());
3209 assertNull(calibrator.getEstimatedSy());
3210 assertNull(calibrator.getEstimatedSz());
3211 assertNull(calibrator.getEstimatedMxy());
3212 assertNull(calibrator.getEstimatedMxz());
3213 assertNull(calibrator.getEstimatedMyx());
3214 assertNull(calibrator.getEstimatedMyz());
3215 assertNull(calibrator.getEstimatedMzx());
3216 assertNull(calibrator.getEstimatedMzy());
3217 assertNull(calibrator.getEstimatedCovariance());
3218 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3219 }
3220
3221 @Test
3222 public void testConstructor32() throws WrongSizeException {
3223 final Matrix ba = generateBa();
3224 final double biasX = ba.getElementAtIndex(0);
3225 final double biasY = ba.getElementAtIndex(1);
3226 final double biasZ = ba.getElementAtIndex(2);
3227
3228 final Matrix ma = generateMaCommonAxis();
3229 final double sx = ma.getElementAt(0, 0);
3230 final double sy = ma.getElementAt(1, 1);
3231 final double sz = ma.getElementAt(2, 2);
3232
3233 final Acceleration bx = new Acceleration(biasX,
3234 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3235 final Acceleration by = new Acceleration(biasY,
3236 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3237 final Acceleration bz = new Acceleration(biasZ,
3238 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3239
3240 final KnownPositionAccelerometerCalibrator calibrator =
3241 new KnownPositionAccelerometerCalibrator(bx, by, bz,
3242 sx, sy, sz);
3243
3244
3245 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3246 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3247 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3248 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3249 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3250 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3251 final Acceleration bx2 = new Acceleration(0.0,
3252 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3253 calibrator.getInitialBiasXAsAcceleration(bx2);
3254 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3255 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3256 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3257 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3258 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3259 final Acceleration by2 = new Acceleration(0.0,
3260 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3261 calibrator.getInitialBiasYAsAcceleration(by2);
3262 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3263 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3264 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3265 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3266 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3267 final Acceleration bz2 = new Acceleration(0.0,
3268 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3269 calibrator.getInitialBiasZAsAcceleration(bz2);
3270 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3271 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3272 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3273 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3274 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3275 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3276 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3277 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3278 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3279 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3280 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3281 final double[] bias1 = calibrator.getInitialBias();
3282 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
3283 final double[] bias2 = new double[3];
3284 calibrator.getInitialBias(bias2);
3285 assertArrayEquals(bias1, bias2, 0.0);
3286 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3287 assertEquals(b1, ba);
3288 final Matrix b2 = new Matrix(3, 1);
3289 calibrator.getInitialBiasAsMatrix(b2);
3290 assertEquals(b1, b2);
3291 final Matrix ma1 = calibrator.getInitialMa();
3292 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
3293 final Matrix ma2 = new Matrix(3, 3);
3294 calibrator.getInitialMa(ma2);
3295 assertEquals(ma1, ma2);
3296 assertNull(calibrator.getEcefPosition());
3297 assertNull(calibrator.getNedPosition());
3298 assertFalse(calibrator.getNedPosition(null));
3299 assertNull(calibrator.getMeasurements());
3300 assertFalse(calibrator.isCommonAxisUsed());
3301 assertNull(calibrator.getListener());
3302 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3303 assertFalse(calibrator.isReady());
3304 assertFalse(calibrator.isRunning());
3305 assertNull(calibrator.getEstimatedBiases());
3306 assertFalse(calibrator.getEstimatedBiases(null));
3307 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3308 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3309 assertNull(calibrator.getEstimatedBiasFx());
3310 assertNull(calibrator.getEstimatedBiasFy());
3311 assertNull(calibrator.getEstimatedBiasFz());
3312 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3313 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3314 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3315 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3316 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3317 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3318 assertNull(calibrator.getEstimatedMa());
3319 assertNull(calibrator.getEstimatedSx());
3320 assertNull(calibrator.getEstimatedSy());
3321 assertNull(calibrator.getEstimatedSz());
3322 assertNull(calibrator.getEstimatedMxy());
3323 assertNull(calibrator.getEstimatedMxz());
3324 assertNull(calibrator.getEstimatedMyx());
3325 assertNull(calibrator.getEstimatedMyz());
3326 assertNull(calibrator.getEstimatedMzx());
3327 assertNull(calibrator.getEstimatedMzy());
3328 assertNull(calibrator.getEstimatedCovariance());
3329 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3330 }
3331
3332 @Test
3333 public void testConstructor33() throws WrongSizeException {
3334 final Matrix ba = generateBa();
3335 final double biasX = ba.getElementAtIndex(0);
3336 final double biasY = ba.getElementAtIndex(1);
3337 final double biasZ = ba.getElementAtIndex(2);
3338
3339 final Matrix ma = generateMaCommonAxis();
3340 final double sx = ma.getElementAt(0, 0);
3341 final double sy = ma.getElementAt(1, 1);
3342 final double sz = ma.getElementAt(2, 2);
3343
3344 final Acceleration bx = new Acceleration(biasX,
3345 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3346 final Acceleration by = new Acceleration(biasY,
3347 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3348 final Acceleration bz = new Acceleration(biasZ,
3349 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3350
3351 final KnownPositionAccelerometerCalibrator calibrator =
3352 new KnownPositionAccelerometerCalibrator(bx, by, bz,
3353 sx, sy, sz, this);
3354
3355
3356 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3357 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3358 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3359 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3360 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3361 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3362 final Acceleration bx2 = new Acceleration(0.0,
3363 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3364 calibrator.getInitialBiasXAsAcceleration(bx2);
3365 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3366 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3367 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3368 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3369 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3370 final Acceleration by2 = new Acceleration(0.0,
3371 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3372 calibrator.getInitialBiasYAsAcceleration(by2);
3373 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3374 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3375 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3376 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3377 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3378 final Acceleration bz2 = new Acceleration(0.0,
3379 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3380 calibrator.getInitialBiasZAsAcceleration(bz2);
3381 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3382 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3383 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3384 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3385 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3386 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3387 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3388 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3389 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3390 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3391 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3392 final double[] bias1 = calibrator.getInitialBias();
3393 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
3394 final double[] bias2 = new double[3];
3395 calibrator.getInitialBias(bias2);
3396 assertArrayEquals(bias1, bias2, 0.0);
3397 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3398 assertEquals(b1, ba);
3399 final Matrix b2 = new Matrix(3, 1);
3400 calibrator.getInitialBiasAsMatrix(b2);
3401 assertEquals(b1, b2);
3402 final Matrix ma1 = calibrator.getInitialMa();
3403 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
3404 final Matrix ma2 = new Matrix(3, 3);
3405 calibrator.getInitialMa(ma2);
3406 assertEquals(ma1, ma2);
3407 assertNull(calibrator.getEcefPosition());
3408 assertNull(calibrator.getNedPosition());
3409 assertFalse(calibrator.getNedPosition(null));
3410 assertNull(calibrator.getMeasurements());
3411 assertFalse(calibrator.isCommonAxisUsed());
3412 assertSame(calibrator.getListener(), this);
3413 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3414 assertFalse(calibrator.isReady());
3415 assertFalse(calibrator.isRunning());
3416 assertNull(calibrator.getEstimatedBiases());
3417 assertFalse(calibrator.getEstimatedBiases(null));
3418 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3419 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3420 assertNull(calibrator.getEstimatedBiasFx());
3421 assertNull(calibrator.getEstimatedBiasFy());
3422 assertNull(calibrator.getEstimatedBiasFz());
3423 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3424 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3425 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3426 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3427 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3428 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3429 assertNull(calibrator.getEstimatedMa());
3430 assertNull(calibrator.getEstimatedSx());
3431 assertNull(calibrator.getEstimatedSy());
3432 assertNull(calibrator.getEstimatedSz());
3433 assertNull(calibrator.getEstimatedMxy());
3434 assertNull(calibrator.getEstimatedMxz());
3435 assertNull(calibrator.getEstimatedMyx());
3436 assertNull(calibrator.getEstimatedMyz());
3437 assertNull(calibrator.getEstimatedMzx());
3438 assertNull(calibrator.getEstimatedMzy());
3439 assertNull(calibrator.getEstimatedCovariance());
3440 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3441 }
3442
3443 @Test
3444 public void testConstructor34() throws WrongSizeException {
3445 final Collection<StandardDeviationBodyKinematics> measurements =
3446 Collections.emptyList();
3447
3448 final Matrix ba = generateBa();
3449 final double biasX = ba.getElementAtIndex(0);
3450 final double biasY = ba.getElementAtIndex(1);
3451 final double biasZ = ba.getElementAtIndex(2);
3452
3453 final Matrix ma = generateMaCommonAxis();
3454 final double sx = ma.getElementAt(0, 0);
3455 final double sy = ma.getElementAt(1, 1);
3456 final double sz = ma.getElementAt(2, 2);
3457
3458 final Acceleration bx = new Acceleration(biasX,
3459 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3460 final Acceleration by = new Acceleration(biasY,
3461 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3462 final Acceleration bz = new Acceleration(biasZ,
3463 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3464
3465 final KnownPositionAccelerometerCalibrator calibrator =
3466 new KnownPositionAccelerometerCalibrator(measurements,
3467 bx, by, bz, sx, sy, sz);
3468
3469
3470 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3471 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3472 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3473 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3474 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3475 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3476 final Acceleration bx2 = new Acceleration(0.0,
3477 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3478 calibrator.getInitialBiasXAsAcceleration(bx2);
3479 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3480 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3481 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3482 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3483 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3484 final Acceleration by2 = new Acceleration(0.0,
3485 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3486 calibrator.getInitialBiasYAsAcceleration(by2);
3487 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3488 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3489 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3490 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3491 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3492 final Acceleration bz2 = new Acceleration(0.0,
3493 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3494 calibrator.getInitialBiasZAsAcceleration(bz2);
3495 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3496 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3497 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3498 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3499 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3500 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3501 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3502 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3503 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3504 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3505 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3506 final double[] bias1 = calibrator.getInitialBias();
3507 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
3508 final double[] bias2 = new double[3];
3509 calibrator.getInitialBias(bias2);
3510 assertArrayEquals(bias1, bias2, 0.0);
3511 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3512 assertEquals(b1, ba);
3513 final Matrix b2 = new Matrix(3, 1);
3514 calibrator.getInitialBiasAsMatrix(b2);
3515 assertEquals(b1, b2);
3516 final Matrix ma1 = calibrator.getInitialMa();
3517 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
3518 final Matrix ma2 = new Matrix(3, 3);
3519 calibrator.getInitialMa(ma2);
3520 assertEquals(ma1, ma2);
3521 assertNull(calibrator.getEcefPosition());
3522 assertNull(calibrator.getNedPosition());
3523 assertFalse(calibrator.getNedPosition(null));
3524 assertSame(calibrator.getMeasurements(), measurements);
3525 assertFalse(calibrator.isCommonAxisUsed());
3526 assertNull(calibrator.getListener());
3527 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3528 assertFalse(calibrator.isReady());
3529 assertFalse(calibrator.isRunning());
3530 assertNull(calibrator.getEstimatedBiases());
3531 assertFalse(calibrator.getEstimatedBiases(null));
3532 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3533 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3534 assertNull(calibrator.getEstimatedBiasFx());
3535 assertNull(calibrator.getEstimatedBiasFy());
3536 assertNull(calibrator.getEstimatedBiasFz());
3537 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3538 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3539 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3540 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3541 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3542 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3543 assertNull(calibrator.getEstimatedMa());
3544 assertNull(calibrator.getEstimatedSx());
3545 assertNull(calibrator.getEstimatedSy());
3546 assertNull(calibrator.getEstimatedSz());
3547 assertNull(calibrator.getEstimatedMxy());
3548 assertNull(calibrator.getEstimatedMxz());
3549 assertNull(calibrator.getEstimatedMyx());
3550 assertNull(calibrator.getEstimatedMyz());
3551 assertNull(calibrator.getEstimatedMzx());
3552 assertNull(calibrator.getEstimatedMzy());
3553 assertNull(calibrator.getEstimatedCovariance());
3554 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3555 }
3556
3557 @Test
3558 public void testConstructor35() throws WrongSizeException {
3559 final Collection<StandardDeviationBodyKinematics> measurements =
3560 Collections.emptyList();
3561
3562 final Matrix ba = generateBa();
3563 final double biasX = ba.getElementAtIndex(0);
3564 final double biasY = ba.getElementAtIndex(1);
3565 final double biasZ = ba.getElementAtIndex(2);
3566
3567 final Matrix ma = generateMaCommonAxis();
3568 final double sx = ma.getElementAt(0, 0);
3569 final double sy = ma.getElementAt(1, 1);
3570 final double sz = ma.getElementAt(2, 2);
3571
3572 final Acceleration bx = new Acceleration(biasX,
3573 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3574 final Acceleration by = new Acceleration(biasY,
3575 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3576 final Acceleration bz = new Acceleration(biasZ,
3577 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3578
3579 final KnownPositionAccelerometerCalibrator calibrator =
3580 new KnownPositionAccelerometerCalibrator(measurements,
3581 bx, by, bz, sx, sy, sz, this);
3582
3583
3584 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3585 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3586 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3587 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3588 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3589 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3590 final Acceleration bx2 = new Acceleration(0.0,
3591 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3592 calibrator.getInitialBiasXAsAcceleration(bx2);
3593 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3594 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3595 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3596 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3597 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3598 final Acceleration by2 = new Acceleration(0.0,
3599 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3600 calibrator.getInitialBiasYAsAcceleration(by2);
3601 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3602 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3603 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3604 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3605 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3606 final Acceleration bz2 = new Acceleration(0.0,
3607 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3608 calibrator.getInitialBiasZAsAcceleration(bz2);
3609 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3610 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3611 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3612 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3613 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3614 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3615 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3616 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3617 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3618 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3619 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3620 final double[] bias1 = calibrator.getInitialBias();
3621 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
3622 final double[] bias2 = new double[3];
3623 calibrator.getInitialBias(bias2);
3624 assertArrayEquals(bias1, bias2, 0.0);
3625 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3626 assertEquals(b1, ba);
3627 final Matrix b2 = new Matrix(3, 1);
3628 calibrator.getInitialBiasAsMatrix(b2);
3629 assertEquals(b1, b2);
3630 final Matrix ma1 = calibrator.getInitialMa();
3631 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
3632 final Matrix ma2 = new Matrix(3, 3);
3633 calibrator.getInitialMa(ma2);
3634 assertEquals(ma1, ma2);
3635 assertNull(calibrator.getEcefPosition());
3636 assertNull(calibrator.getNedPosition());
3637 assertFalse(calibrator.getNedPosition(null));
3638 assertSame(calibrator.getMeasurements(), measurements);
3639 assertFalse(calibrator.isCommonAxisUsed());
3640 assertSame(calibrator.getListener(), this);
3641 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3642 assertFalse(calibrator.isReady());
3643 assertFalse(calibrator.isRunning());
3644 assertNull(calibrator.getEstimatedBiases());
3645 assertFalse(calibrator.getEstimatedBiases(null));
3646 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3647 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3648 assertNull(calibrator.getEstimatedBiasFx());
3649 assertNull(calibrator.getEstimatedBiasFy());
3650 assertNull(calibrator.getEstimatedBiasFz());
3651 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3652 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3653 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3654 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3655 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3656 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3657 assertNull(calibrator.getEstimatedMa());
3658 assertNull(calibrator.getEstimatedSx());
3659 assertNull(calibrator.getEstimatedSy());
3660 assertNull(calibrator.getEstimatedSz());
3661 assertNull(calibrator.getEstimatedMxy());
3662 assertNull(calibrator.getEstimatedMxz());
3663 assertNull(calibrator.getEstimatedMyx());
3664 assertNull(calibrator.getEstimatedMyz());
3665 assertNull(calibrator.getEstimatedMzx());
3666 assertNull(calibrator.getEstimatedMzy());
3667 assertNull(calibrator.getEstimatedCovariance());
3668 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3669 }
3670
3671 @Test
3672 public void testConstructor36() throws WrongSizeException {
3673 final Matrix ba = generateBa();
3674 final double biasX = ba.getElementAtIndex(0);
3675 final double biasY = ba.getElementAtIndex(1);
3676 final double biasZ = ba.getElementAtIndex(2);
3677
3678 final Matrix ma = generateMaCommonAxis();
3679 final double sx = ma.getElementAt(0, 0);
3680 final double sy = ma.getElementAt(1, 1);
3681 final double sz = ma.getElementAt(2, 2);
3682
3683 final Acceleration bx = new Acceleration(biasX,
3684 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3685 final Acceleration by = new Acceleration(biasY,
3686 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3687 final Acceleration bz = new Acceleration(biasZ,
3688 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3689
3690 final KnownPositionAccelerometerCalibrator calibrator =
3691 new KnownPositionAccelerometerCalibrator(true,
3692 bx, by, bz, sx, sy, sz);
3693
3694
3695 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3696 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3697 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3698 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3699 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3700 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3701 final Acceleration bx2 = new Acceleration(0.0,
3702 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3703 calibrator.getInitialBiasXAsAcceleration(bx2);
3704 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3705 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3706 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3707 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3708 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3709 final Acceleration by2 = new Acceleration(0.0,
3710 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3711 calibrator.getInitialBiasYAsAcceleration(by2);
3712 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3713 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3714 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3715 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3716 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3717 final Acceleration bz2 = new Acceleration(0.0,
3718 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3719 calibrator.getInitialBiasZAsAcceleration(bz2);
3720 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3721 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3722 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3723 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3724 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3725 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3726 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3727 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3728 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3729 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3730 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3731 final double[] bias1 = calibrator.getInitialBias();
3732 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
3733 final double[] bias2 = new double[3];
3734 calibrator.getInitialBias(bias2);
3735 assertArrayEquals(bias1, bias2, 0.0);
3736 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3737 assertEquals(b1, ba);
3738 final Matrix b2 = new Matrix(3, 1);
3739 calibrator.getInitialBiasAsMatrix(b2);
3740 assertEquals(b1, b2);
3741 final Matrix ma1 = calibrator.getInitialMa();
3742 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
3743 final Matrix ma2 = new Matrix(3, 3);
3744 calibrator.getInitialMa(ma2);
3745 assertEquals(ma1, ma2);
3746 assertNull(calibrator.getEcefPosition());
3747 assertNull(calibrator.getNedPosition());
3748 assertFalse(calibrator.getNedPosition(null));
3749 assertNull(calibrator.getMeasurements());
3750 assertTrue(calibrator.isCommonAxisUsed());
3751 assertNull(calibrator.getListener());
3752 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3753 assertFalse(calibrator.isReady());
3754 assertFalse(calibrator.isRunning());
3755 assertNull(calibrator.getEstimatedBiases());
3756 assertFalse(calibrator.getEstimatedBiases(null));
3757 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3758 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3759 assertNull(calibrator.getEstimatedBiasFx());
3760 assertNull(calibrator.getEstimatedBiasFy());
3761 assertNull(calibrator.getEstimatedBiasFz());
3762 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3763 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3764 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3765 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3766 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3767 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3768 assertNull(calibrator.getEstimatedMa());
3769 assertNull(calibrator.getEstimatedSx());
3770 assertNull(calibrator.getEstimatedSy());
3771 assertNull(calibrator.getEstimatedSz());
3772 assertNull(calibrator.getEstimatedMxy());
3773 assertNull(calibrator.getEstimatedMxz());
3774 assertNull(calibrator.getEstimatedMyx());
3775 assertNull(calibrator.getEstimatedMyz());
3776 assertNull(calibrator.getEstimatedMzx());
3777 assertNull(calibrator.getEstimatedMzy());
3778 assertNull(calibrator.getEstimatedCovariance());
3779 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3780 }
3781
3782 @Test
3783 public void testConstructor37() throws WrongSizeException {
3784 final Matrix ba = generateBa();
3785 final double biasX = ba.getElementAtIndex(0);
3786 final double biasY = ba.getElementAtIndex(1);
3787 final double biasZ = ba.getElementAtIndex(2);
3788
3789 final Matrix ma = generateMaCommonAxis();
3790 final double sx = ma.getElementAt(0, 0);
3791 final double sy = ma.getElementAt(1, 1);
3792 final double sz = ma.getElementAt(2, 2);
3793
3794 final Acceleration bx = new Acceleration(biasX,
3795 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3796 final Acceleration by = new Acceleration(biasY,
3797 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3798 final Acceleration bz = new Acceleration(biasZ,
3799 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3800
3801 final KnownPositionAccelerometerCalibrator calibrator =
3802 new KnownPositionAccelerometerCalibrator(true,
3803 bx, by, bz, sx, sy, sz, this);
3804
3805
3806 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3807 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3808 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3809 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3810 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3811 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3812 final Acceleration bx2 = new Acceleration(0.0,
3813 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3814 calibrator.getInitialBiasXAsAcceleration(bx2);
3815 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3816 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3817 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3818 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3819 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3820 final Acceleration by2 = new Acceleration(0.0,
3821 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3822 calibrator.getInitialBiasYAsAcceleration(by2);
3823 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3824 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3825 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3826 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3827 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3828 final Acceleration bz2 = new Acceleration(0.0,
3829 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3830 calibrator.getInitialBiasZAsAcceleration(bz2);
3831 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3832 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3833 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3834 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3835 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3836 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3837 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3838 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3839 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3840 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3841 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3842 final double[] bias1 = calibrator.getInitialBias();
3843 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
3844 final double[] bias2 = new double[3];
3845 calibrator.getInitialBias(bias2);
3846 assertArrayEquals(bias1, bias2, 0.0);
3847 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3848 assertEquals(b1, ba);
3849 final Matrix b2 = new Matrix(3, 1);
3850 calibrator.getInitialBiasAsMatrix(b2);
3851 assertEquals(b1, b2);
3852 final Matrix ma1 = calibrator.getInitialMa();
3853 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
3854 final Matrix ma2 = new Matrix(3, 3);
3855 calibrator.getInitialMa(ma2);
3856 assertEquals(ma1, ma2);
3857 assertNull(calibrator.getEcefPosition());
3858 assertNull(calibrator.getNedPosition());
3859 assertFalse(calibrator.getNedPosition(null));
3860 assertNull(calibrator.getMeasurements());
3861 assertTrue(calibrator.isCommonAxisUsed());
3862 assertSame(calibrator.getListener(), this);
3863 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3864 assertFalse(calibrator.isReady());
3865 assertFalse(calibrator.isRunning());
3866 assertNull(calibrator.getEstimatedBiases());
3867 assertFalse(calibrator.getEstimatedBiases(null));
3868 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3869 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3870 assertNull(calibrator.getEstimatedBiasFx());
3871 assertNull(calibrator.getEstimatedBiasFy());
3872 assertNull(calibrator.getEstimatedBiasFz());
3873 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3874 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3875 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3876 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3877 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3878 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3879 assertNull(calibrator.getEstimatedMa());
3880 assertNull(calibrator.getEstimatedSx());
3881 assertNull(calibrator.getEstimatedSy());
3882 assertNull(calibrator.getEstimatedSz());
3883 assertNull(calibrator.getEstimatedMxy());
3884 assertNull(calibrator.getEstimatedMxz());
3885 assertNull(calibrator.getEstimatedMyx());
3886 assertNull(calibrator.getEstimatedMyz());
3887 assertNull(calibrator.getEstimatedMzx());
3888 assertNull(calibrator.getEstimatedMzy());
3889 assertNull(calibrator.getEstimatedCovariance());
3890 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3891 }
3892
3893 @Test
3894 public void testConstructor38() throws WrongSizeException {
3895 final Collection<StandardDeviationBodyKinematics> measurements =
3896 Collections.emptyList();
3897
3898 final Matrix ba = generateBa();
3899 final double biasX = ba.getElementAtIndex(0);
3900 final double biasY = ba.getElementAtIndex(1);
3901 final double biasZ = ba.getElementAtIndex(2);
3902
3903 final Matrix ma = generateMaCommonAxis();
3904 final double sx = ma.getElementAt(0, 0);
3905 final double sy = ma.getElementAt(1, 1);
3906 final double sz = ma.getElementAt(2, 2);
3907
3908 final Acceleration bx = new Acceleration(biasX,
3909 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3910 final Acceleration by = new Acceleration(biasY,
3911 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3912 final Acceleration bz = new Acceleration(biasZ,
3913 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3914
3915 final KnownPositionAccelerometerCalibrator calibrator =
3916 new KnownPositionAccelerometerCalibrator(measurements,
3917 true, bx, by, bz, sx, sy, sz);
3918
3919
3920 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3921 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3922 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3923 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3924 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3925 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3926 final Acceleration bx2 = new Acceleration(0.0,
3927 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3928 calibrator.getInitialBiasXAsAcceleration(bx2);
3929 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3930 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3931 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3932 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3933 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3934 final Acceleration by2 = new Acceleration(0.0,
3935 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3936 calibrator.getInitialBiasYAsAcceleration(by2);
3937 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3938 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3939 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3940 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3941 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3942 final Acceleration bz2 = new Acceleration(0.0,
3943 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3944 calibrator.getInitialBiasZAsAcceleration(bz2);
3945 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3946 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3947 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3948 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3949 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3950 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3951 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3952 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3953 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3954 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3955 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3956 final double[] bias1 = calibrator.getInitialBias();
3957 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
3958 final double[] bias2 = new double[3];
3959 calibrator.getInitialBias(bias2);
3960 assertArrayEquals(bias1, bias2, 0.0);
3961 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3962 assertEquals(b1, ba);
3963 final Matrix b2 = new Matrix(3, 1);
3964 calibrator.getInitialBiasAsMatrix(b2);
3965 assertEquals(b1, b2);
3966 final Matrix ma1 = calibrator.getInitialMa();
3967 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
3968 final Matrix ma2 = new Matrix(3, 3);
3969 calibrator.getInitialMa(ma2);
3970 assertEquals(ma1, ma2);
3971 assertNull(calibrator.getEcefPosition());
3972 assertNull(calibrator.getNedPosition());
3973 assertFalse(calibrator.getNedPosition(null));
3974 assertSame(calibrator.getMeasurements(), measurements);
3975 assertTrue(calibrator.isCommonAxisUsed());
3976 assertNull(calibrator.getListener());
3977 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3978 assertFalse(calibrator.isReady());
3979 assertFalse(calibrator.isRunning());
3980 assertNull(calibrator.getEstimatedBiases());
3981 assertFalse(calibrator.getEstimatedBiases(null));
3982 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3983 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3984 assertNull(calibrator.getEstimatedBiasFx());
3985 assertNull(calibrator.getEstimatedBiasFy());
3986 assertNull(calibrator.getEstimatedBiasFz());
3987 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3988 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3989 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3990 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3991 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3992 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3993 assertNull(calibrator.getEstimatedMa());
3994 assertNull(calibrator.getEstimatedSx());
3995 assertNull(calibrator.getEstimatedSy());
3996 assertNull(calibrator.getEstimatedSz());
3997 assertNull(calibrator.getEstimatedMxy());
3998 assertNull(calibrator.getEstimatedMxz());
3999 assertNull(calibrator.getEstimatedMyx());
4000 assertNull(calibrator.getEstimatedMyz());
4001 assertNull(calibrator.getEstimatedMzx());
4002 assertNull(calibrator.getEstimatedMzy());
4003 assertNull(calibrator.getEstimatedCovariance());
4004 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4005 }
4006
4007 @Test
4008 public void testConstructor39() throws WrongSizeException {
4009 final Collection<StandardDeviationBodyKinematics> measurements =
4010 Collections.emptyList();
4011
4012 final Matrix ba = generateBa();
4013 final double biasX = ba.getElementAtIndex(0);
4014 final double biasY = ba.getElementAtIndex(1);
4015 final double biasZ = ba.getElementAtIndex(2);
4016
4017 final Matrix ma = generateMaCommonAxis();
4018 final double sx = ma.getElementAt(0, 0);
4019 final double sy = ma.getElementAt(1, 1);
4020 final double sz = ma.getElementAt(2, 2);
4021
4022 final Acceleration bx = new Acceleration(biasX,
4023 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4024 final Acceleration by = new Acceleration(biasY,
4025 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4026 final Acceleration bz = new Acceleration(biasZ,
4027 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4028
4029 final KnownPositionAccelerometerCalibrator calibrator =
4030 new KnownPositionAccelerometerCalibrator(measurements,
4031 true, bx, by, bz, sx, sy, sz,
4032 this);
4033
4034
4035 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4036 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4037 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4038 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4039 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4040 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4041 final Acceleration bx2 = new Acceleration(0.0,
4042 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4043 calibrator.getInitialBiasXAsAcceleration(bx2);
4044 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4045 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4046 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4047 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4048 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4049 final Acceleration by2 = new Acceleration(0.0,
4050 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4051 calibrator.getInitialBiasYAsAcceleration(by2);
4052 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4053 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4054 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4055 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4056 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4057 final Acceleration bz2 = new Acceleration(0.0,
4058 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4059 calibrator.getInitialBiasZAsAcceleration(bz2);
4060 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4061 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4062 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4063 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4064 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4065 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
4066 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
4067 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
4068 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
4069 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
4070 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
4071 final double[] bias1 = calibrator.getInitialBias();
4072 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
4073 final double[] bias2 = new double[3];
4074 calibrator.getInitialBias(bias2);
4075 assertArrayEquals(bias1, bias2, 0.0);
4076 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4077 assertEquals(b1, ba);
4078 final Matrix b2 = new Matrix(3, 1);
4079 calibrator.getInitialBiasAsMatrix(b2);
4080 assertEquals(b1, b2);
4081 final Matrix ma1 = calibrator.getInitialMa();
4082 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
4083 final Matrix ma2 = new Matrix(3, 3);
4084 calibrator.getInitialMa(ma2);
4085 assertEquals(ma1, ma2);
4086 assertNull(calibrator.getEcefPosition());
4087 assertNull(calibrator.getNedPosition());
4088 assertFalse(calibrator.getNedPosition(null));
4089 assertSame(calibrator.getMeasurements(), measurements);
4090 assertTrue(calibrator.isCommonAxisUsed());
4091 assertSame(calibrator.getListener(), this);
4092 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4093 assertFalse(calibrator.isReady());
4094 assertFalse(calibrator.isRunning());
4095 assertNull(calibrator.getEstimatedBiases());
4096 assertFalse(calibrator.getEstimatedBiases(null));
4097 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4098 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4099 assertNull(calibrator.getEstimatedBiasFx());
4100 assertNull(calibrator.getEstimatedBiasFy());
4101 assertNull(calibrator.getEstimatedBiasFz());
4102 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4103 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4104 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4105 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4106 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4107 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4108 assertNull(calibrator.getEstimatedMa());
4109 assertNull(calibrator.getEstimatedSx());
4110 assertNull(calibrator.getEstimatedSy());
4111 assertNull(calibrator.getEstimatedSz());
4112 assertNull(calibrator.getEstimatedMxy());
4113 assertNull(calibrator.getEstimatedMxz());
4114 assertNull(calibrator.getEstimatedMyx());
4115 assertNull(calibrator.getEstimatedMyz());
4116 assertNull(calibrator.getEstimatedMzx());
4117 assertNull(calibrator.getEstimatedMzy());
4118 assertNull(calibrator.getEstimatedCovariance());
4119 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4120 }
4121
4122 @Test
4123 public void testConstructor40() throws WrongSizeException {
4124 final Matrix ba = generateBa();
4125 final double biasX = ba.getElementAtIndex(0);
4126 final double biasY = ba.getElementAtIndex(1);
4127 final double biasZ = ba.getElementAtIndex(2);
4128
4129 final Matrix ma = generateMaCommonAxis();
4130 final double sx = ma.getElementAt(0, 0);
4131 final double sy = ma.getElementAt(1, 1);
4132 final double sz = ma.getElementAt(2, 2);
4133 final double mxy = ma.getElementAt(0, 1);
4134 final double mxz = ma.getElementAt(0, 2);
4135 final double myx = ma.getElementAt(1, 0);
4136 final double myz = ma.getElementAt(1, 2);
4137 final double mzx = ma.getElementAt(2, 0);
4138 final double mzy = ma.getElementAt(2, 1);
4139
4140 final KnownPositionAccelerometerCalibrator calibrator =
4141 new KnownPositionAccelerometerCalibrator(biasX, biasY, biasZ,
4142 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
4143
4144
4145 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4146 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4147 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4148 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4149 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4150 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4151 final Acceleration bx2 = new Acceleration(0.0,
4152 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4153 calibrator.getInitialBiasXAsAcceleration(bx2);
4154 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4155 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4156 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4157 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4158 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4159 final Acceleration by2 = new Acceleration(0.0,
4160 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4161 calibrator.getInitialBiasYAsAcceleration(by2);
4162 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4163 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4164 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4165 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4166 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4167 final Acceleration bz2 = new Acceleration(0.0,
4168 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4169 calibrator.getInitialBiasZAsAcceleration(bz2);
4170 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4171 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4172 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4173 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4174 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4175 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4176 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4177 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4178 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4179 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4180 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4181 final double[] bias1 = calibrator.getInitialBias();
4182 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
4183 final double[] bias2 = new double[3];
4184 calibrator.getInitialBias(bias2);
4185 assertArrayEquals(bias1, bias2, 0.0);
4186 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4187 assertEquals(b1, ba);
4188 final Matrix b2 = new Matrix(3, 1);
4189 calibrator.getInitialBiasAsMatrix(b2);
4190 assertEquals(b1, b2);
4191 final Matrix ma1 = new Matrix(3, 3);
4192 ma1.setSubmatrix(0, 0,
4193 2, 2,
4194 new double[]{ sx, myx, mzx,
4195 mxy, sy, mzy,
4196 mxz, myz, sz});
4197 assertEquals(calibrator.getInitialMa(), ma1);
4198 final Matrix ma2 = new Matrix(3, 3);
4199 calibrator.getInitialMa(ma2);
4200 assertEquals(ma1, ma2);
4201 assertNull(calibrator.getEcefPosition());
4202 assertNull(calibrator.getNedPosition());
4203 assertFalse(calibrator.getNedPosition(null));
4204 assertNull(calibrator.getMeasurements());
4205 assertFalse(calibrator.isCommonAxisUsed());
4206 assertNull(calibrator.getListener());
4207 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
4208 assertFalse(calibrator.isReady());
4209 assertFalse(calibrator.isRunning());
4210 assertNull(calibrator.getEstimatedBiases());
4211 assertFalse(calibrator.getEstimatedBiases(null));
4212 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4213 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4214 assertNull(calibrator.getEstimatedBiasFx());
4215 assertNull(calibrator.getEstimatedBiasFy());
4216 assertNull(calibrator.getEstimatedBiasFz());
4217 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4218 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4219 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4220 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4221 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4222 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4223 assertNull(calibrator.getEstimatedMa());
4224 assertNull(calibrator.getEstimatedSx());
4225 assertNull(calibrator.getEstimatedSy());
4226 assertNull(calibrator.getEstimatedSz());
4227 assertNull(calibrator.getEstimatedMxy());
4228 assertNull(calibrator.getEstimatedMxz());
4229 assertNull(calibrator.getEstimatedMyx());
4230 assertNull(calibrator.getEstimatedMyz());
4231 assertNull(calibrator.getEstimatedMzx());
4232 assertNull(calibrator.getEstimatedMzy());
4233 assertNull(calibrator.getEstimatedCovariance());
4234 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4235 }
4236
4237 @Test
4238 public void testConstructor41() throws WrongSizeException {
4239 final Collection<StandardDeviationBodyKinematics> measurements =
4240 Collections.emptyList();
4241
4242 final Matrix ba = generateBa();
4243 final double biasX = ba.getElementAtIndex(0);
4244 final double biasY = ba.getElementAtIndex(1);
4245 final double biasZ = ba.getElementAtIndex(2);
4246
4247 final Matrix ma = generateMaCommonAxis();
4248 final double sx = ma.getElementAt(0, 0);
4249 final double sy = ma.getElementAt(1, 1);
4250 final double sz = ma.getElementAt(2, 2);
4251 final double mxy = ma.getElementAt(0, 1);
4252 final double mxz = ma.getElementAt(0, 2);
4253 final double myx = ma.getElementAt(1, 0);
4254 final double myz = ma.getElementAt(1, 2);
4255 final double mzx = ma.getElementAt(2, 0);
4256 final double mzy = ma.getElementAt(2, 1);
4257
4258 final KnownPositionAccelerometerCalibrator calibrator =
4259 new KnownPositionAccelerometerCalibrator(measurements,
4260 biasX, biasY, biasZ, sx, sy, sz,
4261 mxy, mxz, myx, myz, mzx, mzy);
4262
4263
4264 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4265 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4266 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4267 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4268 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4269 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4270 final Acceleration bx2 = new Acceleration(0.0,
4271 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4272 calibrator.getInitialBiasXAsAcceleration(bx2);
4273 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4274 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4275 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4276 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4277 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4278 final Acceleration by2 = new Acceleration(0.0,
4279 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4280 calibrator.getInitialBiasYAsAcceleration(by2);
4281 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4282 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4283 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4284 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4285 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4286 final Acceleration bz2 = new Acceleration(0.0,
4287 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4288 calibrator.getInitialBiasZAsAcceleration(bz2);
4289 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4290 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4291 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4292 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4293 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4294 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4295 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4296 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4297 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4298 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4299 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4300 final double[] bias1 = calibrator.getInitialBias();
4301 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
4302 final double[] bias2 = new double[3];
4303 calibrator.getInitialBias(bias2);
4304 assertArrayEquals(bias1, bias2, 0.0);
4305 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4306 assertEquals(b1, ba);
4307 final Matrix b2 = new Matrix(3, 1);
4308 calibrator.getInitialBiasAsMatrix(b2);
4309 assertEquals(b1, b2);
4310 final Matrix ma1 = new Matrix(3, 3);
4311 ma1.setSubmatrix(0, 0,
4312 2, 2,
4313 new double[]{ sx, myx, mzx,
4314 mxy, sy, mzy,
4315 mxz, myz, sz});
4316 assertEquals(calibrator.getInitialMa(), ma1);
4317 final Matrix ma2 = new Matrix(3, 3);
4318 calibrator.getInitialMa(ma2);
4319 assertEquals(ma1, ma2);
4320 assertNull(calibrator.getEcefPosition());
4321 assertNull(calibrator.getNedPosition());
4322 assertFalse(calibrator.getNedPosition(null));
4323 assertSame(calibrator.getMeasurements(), measurements);
4324 assertFalse(calibrator.isCommonAxisUsed());
4325 assertNull(calibrator.getListener());
4326 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
4327 assertFalse(calibrator.isReady());
4328 assertFalse(calibrator.isRunning());
4329 assertNull(calibrator.getEstimatedBiases());
4330 assertFalse(calibrator.getEstimatedBiases(null));
4331 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4332 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4333 assertNull(calibrator.getEstimatedBiasFx());
4334 assertNull(calibrator.getEstimatedBiasFy());
4335 assertNull(calibrator.getEstimatedBiasFz());
4336 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4337 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4338 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4339 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4340 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4341 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4342 assertNull(calibrator.getEstimatedMa());
4343 assertNull(calibrator.getEstimatedSx());
4344 assertNull(calibrator.getEstimatedSy());
4345 assertNull(calibrator.getEstimatedSz());
4346 assertNull(calibrator.getEstimatedMxy());
4347 assertNull(calibrator.getEstimatedMxz());
4348 assertNull(calibrator.getEstimatedMyx());
4349 assertNull(calibrator.getEstimatedMyz());
4350 assertNull(calibrator.getEstimatedMzx());
4351 assertNull(calibrator.getEstimatedMzy());
4352 assertNull(calibrator.getEstimatedCovariance());
4353 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4354 }
4355
4356 @Test
4357 public void testConstructor42() throws WrongSizeException {
4358 final Collection<StandardDeviationBodyKinematics> measurements =
4359 Collections.emptyList();
4360
4361 final Matrix ba = generateBa();
4362 final double biasX = ba.getElementAtIndex(0);
4363 final double biasY = ba.getElementAtIndex(1);
4364 final double biasZ = ba.getElementAtIndex(2);
4365
4366 final Matrix ma = generateMaCommonAxis();
4367 final double sx = ma.getElementAt(0, 0);
4368 final double sy = ma.getElementAt(1, 1);
4369 final double sz = ma.getElementAt(2, 2);
4370 final double mxy = ma.getElementAt(0, 1);
4371 final double mxz = ma.getElementAt(0, 2);
4372 final double myx = ma.getElementAt(1, 0);
4373 final double myz = ma.getElementAt(1, 2);
4374 final double mzx = ma.getElementAt(2, 0);
4375 final double mzy = ma.getElementAt(2, 1);
4376
4377 final KnownPositionAccelerometerCalibrator calibrator =
4378 new KnownPositionAccelerometerCalibrator(measurements,
4379 biasX, biasY, biasZ, sx, sy, sz,
4380 mxy, mxz, myx, myz, mzx, mzy, this);
4381
4382
4383 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4384 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4385 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4386 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4387 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4388 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4389 final Acceleration bx2 = new Acceleration(0.0,
4390 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4391 calibrator.getInitialBiasXAsAcceleration(bx2);
4392 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4393 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4394 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4395 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4396 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4397 final Acceleration by2 = new Acceleration(0.0,
4398 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4399 calibrator.getInitialBiasYAsAcceleration(by2);
4400 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4401 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4402 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4403 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4404 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4405 final Acceleration bz2 = new Acceleration(0.0,
4406 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4407 calibrator.getInitialBiasZAsAcceleration(bz2);
4408 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4409 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4410 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4411 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4412 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4413 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4414 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4415 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4416 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4417 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4418 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4419 final double[] bias1 = calibrator.getInitialBias();
4420 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
4421 final double[] bias2 = new double[3];
4422 calibrator.getInitialBias(bias2);
4423 assertArrayEquals(bias1, bias2, 0.0);
4424 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4425 assertEquals(b1, ba);
4426 final Matrix b2 = new Matrix(3, 1);
4427 calibrator.getInitialBiasAsMatrix(b2);
4428 assertEquals(b1, b2);
4429 final Matrix ma1 = new Matrix(3, 3);
4430 ma1.setSubmatrix(0, 0,
4431 2, 2,
4432 new double[]{ sx, myx, mzx,
4433 mxy, sy, mzy,
4434 mxz, myz, sz});
4435 assertEquals(calibrator.getInitialMa(), ma1);
4436 final Matrix ma2 = new Matrix(3, 3);
4437 calibrator.getInitialMa(ma2);
4438 assertEquals(ma1, ma2);
4439 assertNull(calibrator.getEcefPosition());
4440 assertNull(calibrator.getNedPosition());
4441 assertFalse(calibrator.getNedPosition(null));
4442 assertSame(calibrator.getMeasurements(), measurements);
4443 assertFalse(calibrator.isCommonAxisUsed());
4444 assertSame(calibrator.getListener(), this);
4445 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
4446 assertFalse(calibrator.isReady());
4447 assertFalse(calibrator.isRunning());
4448 assertNull(calibrator.getEstimatedBiases());
4449 assertFalse(calibrator.getEstimatedBiases(null));
4450 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4451 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4452 assertNull(calibrator.getEstimatedBiasFx());
4453 assertNull(calibrator.getEstimatedBiasFy());
4454 assertNull(calibrator.getEstimatedBiasFz());
4455 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4456 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4457 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4458 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4459 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4460 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4461 assertNull(calibrator.getEstimatedMa());
4462 assertNull(calibrator.getEstimatedSx());
4463 assertNull(calibrator.getEstimatedSy());
4464 assertNull(calibrator.getEstimatedSz());
4465 assertNull(calibrator.getEstimatedMxy());
4466 assertNull(calibrator.getEstimatedMxz());
4467 assertNull(calibrator.getEstimatedMyx());
4468 assertNull(calibrator.getEstimatedMyz());
4469 assertNull(calibrator.getEstimatedMzx());
4470 assertNull(calibrator.getEstimatedMzy());
4471 assertNull(calibrator.getEstimatedCovariance());
4472 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4473 }
4474
4475 @Test
4476 public void testConstructor43() throws WrongSizeException {
4477 final Matrix ba = generateBa();
4478 final double biasX = ba.getElementAtIndex(0);
4479 final double biasY = ba.getElementAtIndex(1);
4480 final double biasZ = ba.getElementAtIndex(2);
4481
4482 final Matrix ma = generateMaCommonAxis();
4483 final double sx = ma.getElementAt(0, 0);
4484 final double sy = ma.getElementAt(1, 1);
4485 final double sz = ma.getElementAt(2, 2);
4486 final double mxy = ma.getElementAt(0, 1);
4487 final double mxz = ma.getElementAt(0, 2);
4488 final double myx = ma.getElementAt(1, 0);
4489 final double myz = ma.getElementAt(1, 2);
4490 final double mzx = ma.getElementAt(2, 0);
4491 final double mzy = ma.getElementAt(2, 1);
4492
4493 final KnownPositionAccelerometerCalibrator calibrator =
4494 new KnownPositionAccelerometerCalibrator(true,
4495 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx,
4496 myz, mzx, mzy);
4497
4498
4499 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4500 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4501 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4502 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4503 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4504 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4505 final Acceleration bx2 = new Acceleration(0.0,
4506 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4507 calibrator.getInitialBiasXAsAcceleration(bx2);
4508 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4509 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4510 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4511 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4512 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4513 final Acceleration by2 = new Acceleration(0.0,
4514 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4515 calibrator.getInitialBiasYAsAcceleration(by2);
4516 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4517 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4518 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4519 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4520 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4521 final Acceleration bz2 = new Acceleration(0.0,
4522 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4523 calibrator.getInitialBiasZAsAcceleration(bz2);
4524 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4525 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4526 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4527 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4528 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4529 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4530 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4531 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4532 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4533 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4534 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4535 final double[] bias1 = calibrator.getInitialBias();
4536 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
4537 final double[] bias2 = new double[3];
4538 calibrator.getInitialBias(bias2);
4539 assertArrayEquals(bias1, bias2, 0.0);
4540 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4541 assertEquals(b1, ba);
4542 final Matrix b2 = new Matrix(3, 1);
4543 calibrator.getInitialBiasAsMatrix(b2);
4544 assertEquals(b1, b2);
4545 final Matrix ma1 = new Matrix(3, 3);
4546 ma1.setSubmatrix(0, 0,
4547 2, 2,
4548 new double[]{ sx, myx, mzx,
4549 mxy, sy, mzy,
4550 mxz, myz, sz});
4551 assertEquals(calibrator.getInitialMa(), ma1);
4552 final Matrix ma2 = new Matrix(3, 3);
4553 calibrator.getInitialMa(ma2);
4554 assertEquals(ma1, ma2);
4555 assertNull(calibrator.getEcefPosition());
4556 assertNull(calibrator.getNedPosition());
4557 assertFalse(calibrator.getNedPosition(null));
4558 assertNull(calibrator.getMeasurements());
4559 assertTrue(calibrator.isCommonAxisUsed());
4560 assertNull(calibrator.getListener());
4561 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4562 assertFalse(calibrator.isReady());
4563 assertFalse(calibrator.isRunning());
4564 assertNull(calibrator.getEstimatedBiases());
4565 assertFalse(calibrator.getEstimatedBiases(null));
4566 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4567 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4568 assertNull(calibrator.getEstimatedBiasFx());
4569 assertNull(calibrator.getEstimatedBiasFy());
4570 assertNull(calibrator.getEstimatedBiasFz());
4571 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4572 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4573 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4574 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4575 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4576 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4577 assertNull(calibrator.getEstimatedMa());
4578 assertNull(calibrator.getEstimatedSx());
4579 assertNull(calibrator.getEstimatedSy());
4580 assertNull(calibrator.getEstimatedSz());
4581 assertNull(calibrator.getEstimatedMxy());
4582 assertNull(calibrator.getEstimatedMxz());
4583 assertNull(calibrator.getEstimatedMyx());
4584 assertNull(calibrator.getEstimatedMyz());
4585 assertNull(calibrator.getEstimatedMzx());
4586 assertNull(calibrator.getEstimatedMzy());
4587 assertNull(calibrator.getEstimatedCovariance());
4588 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4589 }
4590
4591 @Test
4592 public void testConstructor44() throws WrongSizeException {
4593 final Matrix ba = generateBa();
4594 final double biasX = ba.getElementAtIndex(0);
4595 final double biasY = ba.getElementAtIndex(1);
4596 final double biasZ = ba.getElementAtIndex(2);
4597
4598 final Matrix ma = generateMaCommonAxis();
4599 final double sx = ma.getElementAt(0, 0);
4600 final double sy = ma.getElementAt(1, 1);
4601 final double sz = ma.getElementAt(2, 2);
4602 final double mxy = ma.getElementAt(0, 1);
4603 final double mxz = ma.getElementAt(0, 2);
4604 final double myx = ma.getElementAt(1, 0);
4605 final double myz = ma.getElementAt(1, 2);
4606 final double mzx = ma.getElementAt(2, 0);
4607 final double mzy = ma.getElementAt(2, 1);
4608
4609 final KnownPositionAccelerometerCalibrator calibrator =
4610 new KnownPositionAccelerometerCalibrator(true,
4611 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx,
4612 myz, mzx, mzy, this);
4613
4614
4615 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4616 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4617 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4618 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4619 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4620 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4621 final Acceleration bx2 = new Acceleration(0.0,
4622 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4623 calibrator.getInitialBiasXAsAcceleration(bx2);
4624 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4625 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4626 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4627 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4628 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4629 final Acceleration by2 = new Acceleration(0.0,
4630 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4631 calibrator.getInitialBiasYAsAcceleration(by2);
4632 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4633 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4634 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4635 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4636 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4637 final Acceleration bz2 = new Acceleration(0.0,
4638 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4639 calibrator.getInitialBiasZAsAcceleration(bz2);
4640 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4641 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4642 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4643 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4644 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4645 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4646 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4647 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4648 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4649 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4650 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4651 final double[] bias1 = calibrator.getInitialBias();
4652 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
4653 final double[] bias2 = new double[3];
4654 calibrator.getInitialBias(bias2);
4655 assertArrayEquals(bias1, bias2, 0.0);
4656 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4657 assertEquals(b1, ba);
4658 final Matrix b2 = new Matrix(3, 1);
4659 calibrator.getInitialBiasAsMatrix(b2);
4660 assertEquals(b1, b2);
4661 final Matrix ma1 = new Matrix(3, 3);
4662 ma1.setSubmatrix(0, 0,
4663 2, 2,
4664 new double[]{ sx, myx, mzx,
4665 mxy, sy, mzy,
4666 mxz, myz, sz});
4667 assertEquals(calibrator.getInitialMa(), ma1);
4668 final Matrix ma2 = new Matrix(3, 3);
4669 calibrator.getInitialMa(ma2);
4670 assertEquals(ma1, ma2);
4671 assertNull(calibrator.getEcefPosition());
4672 assertNull(calibrator.getNedPosition());
4673 assertFalse(calibrator.getNedPosition(null));
4674 assertNull(calibrator.getMeasurements());
4675 assertTrue(calibrator.isCommonAxisUsed());
4676 assertSame(calibrator.getListener(), this);
4677 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4678 assertFalse(calibrator.isReady());
4679 assertFalse(calibrator.isRunning());
4680 assertNull(calibrator.getEstimatedBiases());
4681 assertFalse(calibrator.getEstimatedBiases(null));
4682 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4683 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4684 assertNull(calibrator.getEstimatedBiasFx());
4685 assertNull(calibrator.getEstimatedBiasFy());
4686 assertNull(calibrator.getEstimatedBiasFz());
4687 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4688 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4689 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4690 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4691 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4692 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4693 assertNull(calibrator.getEstimatedMa());
4694 assertNull(calibrator.getEstimatedSx());
4695 assertNull(calibrator.getEstimatedSy());
4696 assertNull(calibrator.getEstimatedSz());
4697 assertNull(calibrator.getEstimatedMxy());
4698 assertNull(calibrator.getEstimatedMxz());
4699 assertNull(calibrator.getEstimatedMyx());
4700 assertNull(calibrator.getEstimatedMyz());
4701 assertNull(calibrator.getEstimatedMzx());
4702 assertNull(calibrator.getEstimatedMzy());
4703 assertNull(calibrator.getEstimatedCovariance());
4704 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4705 }
4706
4707 @Test
4708 public void testConstructor45() throws WrongSizeException {
4709 final Collection<StandardDeviationBodyKinematics> measurements =
4710 Collections.emptyList();
4711
4712 final Matrix ba = generateBa();
4713 final double biasX = ba.getElementAtIndex(0);
4714 final double biasY = ba.getElementAtIndex(1);
4715 final double biasZ = ba.getElementAtIndex(2);
4716
4717 final Matrix ma = generateMaCommonAxis();
4718 final double sx = ma.getElementAt(0, 0);
4719 final double sy = ma.getElementAt(1, 1);
4720 final double sz = ma.getElementAt(2, 2);
4721 final double mxy = ma.getElementAt(0, 1);
4722 final double mxz = ma.getElementAt(0, 2);
4723 final double myx = ma.getElementAt(1, 0);
4724 final double myz = ma.getElementAt(1, 2);
4725 final double mzx = ma.getElementAt(2, 0);
4726 final double mzy = ma.getElementAt(2, 1);
4727
4728 final KnownPositionAccelerometerCalibrator calibrator =
4729 new KnownPositionAccelerometerCalibrator(measurements,
4730 true, biasX, biasY, biasZ,
4731 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
4732
4733
4734 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4735 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4736 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4737 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4738 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4739 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4740 final Acceleration bx2 = new Acceleration(0.0,
4741 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4742 calibrator.getInitialBiasXAsAcceleration(bx2);
4743 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4744 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4745 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4746 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4747 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4748 final Acceleration by2 = new Acceleration(0.0,
4749 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4750 calibrator.getInitialBiasYAsAcceleration(by2);
4751 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4752 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4753 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4754 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4755 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4756 final Acceleration bz2 = new Acceleration(0.0,
4757 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4758 calibrator.getInitialBiasZAsAcceleration(bz2);
4759 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4760 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4761 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4762 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4763 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4764 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4765 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4766 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4767 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4768 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4769 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4770 final double[] bias1 = calibrator.getInitialBias();
4771 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
4772 final double[] bias2 = new double[3];
4773 calibrator.getInitialBias(bias2);
4774 assertArrayEquals(bias1, bias2, 0.0);
4775 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4776 assertEquals(b1, ba);
4777 final Matrix b2 = new Matrix(3, 1);
4778 calibrator.getInitialBiasAsMatrix(b2);
4779 assertEquals(b1, b2);
4780 final Matrix ma1 = new Matrix(3, 3);
4781 ma1.setSubmatrix(0, 0,
4782 2, 2,
4783 new double[]{ sx, myx, mzx,
4784 mxy, sy, mzy,
4785 mxz, myz, sz});
4786 assertEquals(calibrator.getInitialMa(), ma1);
4787 final Matrix ma2 = new Matrix(3, 3);
4788 calibrator.getInitialMa(ma2);
4789 assertEquals(ma1, ma2);
4790 assertNull(calibrator.getEcefPosition());
4791 assertNull(calibrator.getNedPosition());
4792 assertFalse(calibrator.getNedPosition(null));
4793 assertSame(calibrator.getMeasurements(), measurements);
4794 assertTrue(calibrator.isCommonAxisUsed());
4795 assertNull(calibrator.getListener());
4796 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4797 assertFalse(calibrator.isReady());
4798 assertFalse(calibrator.isRunning());
4799 assertNull(calibrator.getEstimatedBiases());
4800 assertFalse(calibrator.getEstimatedBiases(null));
4801 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4802 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4803 assertNull(calibrator.getEstimatedBiasFx());
4804 assertNull(calibrator.getEstimatedBiasFy());
4805 assertNull(calibrator.getEstimatedBiasFz());
4806 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4807 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4808 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4809 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4810 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4811 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4812 assertNull(calibrator.getEstimatedMa());
4813 assertNull(calibrator.getEstimatedSx());
4814 assertNull(calibrator.getEstimatedSy());
4815 assertNull(calibrator.getEstimatedSz());
4816 assertNull(calibrator.getEstimatedMxy());
4817 assertNull(calibrator.getEstimatedMxz());
4818 assertNull(calibrator.getEstimatedMyx());
4819 assertNull(calibrator.getEstimatedMyz());
4820 assertNull(calibrator.getEstimatedMzx());
4821 assertNull(calibrator.getEstimatedMzy());
4822 assertNull(calibrator.getEstimatedCovariance());
4823 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4824 }
4825
4826 @Test
4827 public void testConstructor46() throws WrongSizeException {
4828 final Collection<StandardDeviationBodyKinematics> measurements =
4829 Collections.emptyList();
4830
4831 final Matrix ba = generateBa();
4832 final double biasX = ba.getElementAtIndex(0);
4833 final double biasY = ba.getElementAtIndex(1);
4834 final double biasZ = ba.getElementAtIndex(2);
4835
4836 final Matrix ma = generateMaCommonAxis();
4837 final double sx = ma.getElementAt(0, 0);
4838 final double sy = ma.getElementAt(1, 1);
4839 final double sz = ma.getElementAt(2, 2);
4840 final double mxy = ma.getElementAt(0, 1);
4841 final double mxz = ma.getElementAt(0, 2);
4842 final double myx = ma.getElementAt(1, 0);
4843 final double myz = ma.getElementAt(1, 2);
4844 final double mzx = ma.getElementAt(2, 0);
4845 final double mzy = ma.getElementAt(2, 1);
4846
4847 final KnownPositionAccelerometerCalibrator calibrator =
4848 new KnownPositionAccelerometerCalibrator(measurements,
4849 true, biasX, biasY, biasZ,
4850 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
4851
4852
4853 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4854 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4855 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4856 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4857 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4858 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4859 final Acceleration bx2 = new Acceleration(0.0,
4860 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4861 calibrator.getInitialBiasXAsAcceleration(bx2);
4862 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4863 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4864 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4865 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4866 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4867 final Acceleration by2 = new Acceleration(0.0,
4868 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4869 calibrator.getInitialBiasYAsAcceleration(by2);
4870 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4871 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4872 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4873 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4874 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4875 final Acceleration bz2 = new Acceleration(0.0,
4876 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4877 calibrator.getInitialBiasZAsAcceleration(bz2);
4878 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4879 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4880 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4881 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4882 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4883 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4884 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4885 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4886 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4887 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4888 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4889 final double[] bias1 = calibrator.getInitialBias();
4890 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
4891 final double[] bias2 = new double[3];
4892 calibrator.getInitialBias(bias2);
4893 assertArrayEquals(bias1, bias2, 0.0);
4894 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4895 assertEquals(b1, ba);
4896 final Matrix b2 = new Matrix(3, 1);
4897 calibrator.getInitialBiasAsMatrix(b2);
4898 assertEquals(b1, b2);
4899 final Matrix ma1 = new Matrix(3, 3);
4900 ma1.setSubmatrix(0, 0,
4901 2, 2,
4902 new double[]{ sx, myx, mzx,
4903 mxy, sy, mzy,
4904 mxz, myz, sz});
4905 assertEquals(calibrator.getInitialMa(), ma1);
4906 final Matrix ma2 = new Matrix(3, 3);
4907 calibrator.getInitialMa(ma2);
4908 assertEquals(ma1, ma2);
4909 assertNull(calibrator.getEcefPosition());
4910 assertNull(calibrator.getNedPosition());
4911 assertFalse(calibrator.getNedPosition(null));
4912 assertSame(calibrator.getMeasurements(), measurements);
4913 assertTrue(calibrator.isCommonAxisUsed());
4914 assertSame(calibrator.getListener(), this);
4915 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4916 assertFalse(calibrator.isReady());
4917 assertFalse(calibrator.isRunning());
4918 assertNull(calibrator.getEstimatedBiases());
4919 assertFalse(calibrator.getEstimatedBiases(null));
4920 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4921 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4922 assertNull(calibrator.getEstimatedBiasFx());
4923 assertNull(calibrator.getEstimatedBiasFy());
4924 assertNull(calibrator.getEstimatedBiasFz());
4925 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4926 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4927 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4928 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4929 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4930 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4931 assertNull(calibrator.getEstimatedMa());
4932 assertNull(calibrator.getEstimatedSx());
4933 assertNull(calibrator.getEstimatedSy());
4934 assertNull(calibrator.getEstimatedSz());
4935 assertNull(calibrator.getEstimatedMxy());
4936 assertNull(calibrator.getEstimatedMxz());
4937 assertNull(calibrator.getEstimatedMyx());
4938 assertNull(calibrator.getEstimatedMyz());
4939 assertNull(calibrator.getEstimatedMzx());
4940 assertNull(calibrator.getEstimatedMzy());
4941 assertNull(calibrator.getEstimatedCovariance());
4942 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4943 }
4944
4945 @Test
4946 public void testConstructor47() throws WrongSizeException {
4947 final Matrix ba = generateBa();
4948 final double biasX = ba.getElementAtIndex(0);
4949 final double biasY = ba.getElementAtIndex(1);
4950 final double biasZ = ba.getElementAtIndex(2);
4951
4952 final Matrix ma = generateMaCommonAxis();
4953 final double sx = ma.getElementAt(0, 0);
4954 final double sy = ma.getElementAt(1, 1);
4955 final double sz = ma.getElementAt(2, 2);
4956 final double mxy = ma.getElementAt(0, 1);
4957 final double mxz = ma.getElementAt(0, 2);
4958 final double myx = ma.getElementAt(1, 0);
4959 final double myz = ma.getElementAt(1, 2);
4960 final double mzx = ma.getElementAt(2, 0);
4961 final double mzy = ma.getElementAt(2, 1);
4962
4963 final Acceleration bx = new Acceleration(biasX,
4964 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4965 final Acceleration by = new Acceleration(biasY,
4966 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4967 final Acceleration bz = new Acceleration(biasZ,
4968 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4969
4970 final KnownPositionAccelerometerCalibrator calibrator =
4971 new KnownPositionAccelerometerCalibrator(bx, by, bz,
4972 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
4973
4974
4975 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4976 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4977 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4978 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4979 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4980 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4981 final Acceleration bx2 = new Acceleration(0.0,
4982 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4983 calibrator.getInitialBiasXAsAcceleration(bx2);
4984 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4985 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4986 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4987 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4988 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4989 final Acceleration by2 = new Acceleration(0.0,
4990 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4991 calibrator.getInitialBiasYAsAcceleration(by2);
4992 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4993 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4994 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4995 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4996 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4997 final Acceleration bz2 = new Acceleration(0.0,
4998 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4999 calibrator.getInitialBiasZAsAcceleration(bz2);
5000 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5001 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5002 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5003 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5004 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5005 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5006 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5007 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5008 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5009 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5010 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5011 final double[] bias1 = calibrator.getInitialBias();
5012 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
5013 final double[] bias2 = new double[3];
5014 calibrator.getInitialBias(bias2);
5015 assertArrayEquals(bias1, bias2, 0.0);
5016 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5017 assertEquals(b1, ba);
5018 final Matrix b2 = new Matrix(3, 1);
5019 calibrator.getInitialBiasAsMatrix(b2);
5020 assertEquals(b1, b2);
5021 final Matrix ma1 = new Matrix(3, 3);
5022 ma1.setSubmatrix(0, 0,
5023 2, 2,
5024 new double[]{ sx, myx, mzx,
5025 mxy, sy, mzy,
5026 mxz, myz, sz});
5027 assertEquals(calibrator.getInitialMa(), ma1);
5028 final Matrix ma2 = new Matrix(3, 3);
5029 calibrator.getInitialMa(ma2);
5030 assertEquals(ma1, ma2);
5031 assertNull(calibrator.getEcefPosition());
5032 assertNull(calibrator.getNedPosition());
5033 assertFalse(calibrator.getNedPosition(null));
5034 assertNull(calibrator.getMeasurements());
5035 assertFalse(calibrator.isCommonAxisUsed());
5036 assertNull(calibrator.getListener());
5037 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5038 assertFalse(calibrator.isReady());
5039 assertFalse(calibrator.isRunning());
5040 assertNull(calibrator.getEstimatedBiases());
5041 assertFalse(calibrator.getEstimatedBiases(null));
5042 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5043 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5044 assertNull(calibrator.getEstimatedBiasFx());
5045 assertNull(calibrator.getEstimatedBiasFy());
5046 assertNull(calibrator.getEstimatedBiasFz());
5047 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5048 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5049 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5050 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5051 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5052 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5053 assertNull(calibrator.getEstimatedMa());
5054 assertNull(calibrator.getEstimatedSx());
5055 assertNull(calibrator.getEstimatedSy());
5056 assertNull(calibrator.getEstimatedSz());
5057 assertNull(calibrator.getEstimatedMxy());
5058 assertNull(calibrator.getEstimatedMxz());
5059 assertNull(calibrator.getEstimatedMyx());
5060 assertNull(calibrator.getEstimatedMyz());
5061 assertNull(calibrator.getEstimatedMzx());
5062 assertNull(calibrator.getEstimatedMzy());
5063 assertNull(calibrator.getEstimatedCovariance());
5064 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5065 }
5066
5067 @Test
5068 public void testConstructor48() throws WrongSizeException {
5069 final Matrix ba = generateBa();
5070 final double biasX = ba.getElementAtIndex(0);
5071 final double biasY = ba.getElementAtIndex(1);
5072 final double biasZ = ba.getElementAtIndex(2);
5073
5074 final Matrix ma = generateMaCommonAxis();
5075 final double sx = ma.getElementAt(0, 0);
5076 final double sy = ma.getElementAt(1, 1);
5077 final double sz = ma.getElementAt(2, 2);
5078 final double mxy = ma.getElementAt(0, 1);
5079 final double mxz = ma.getElementAt(0, 2);
5080 final double myx = ma.getElementAt(1, 0);
5081 final double myz = ma.getElementAt(1, 2);
5082 final double mzx = ma.getElementAt(2, 0);
5083 final double mzy = ma.getElementAt(2, 1);
5084
5085 final Acceleration bx = new Acceleration(biasX,
5086 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5087 final Acceleration by = new Acceleration(biasY,
5088 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5089 final Acceleration bz = new Acceleration(biasZ,
5090 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5091
5092 final KnownPositionAccelerometerCalibrator calibrator =
5093 new KnownPositionAccelerometerCalibrator(bx, by, bz,
5094 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
5095
5096
5097 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5098 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5099 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5100 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5101 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5102 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5103 final Acceleration bx2 = new Acceleration(0.0,
5104 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5105 calibrator.getInitialBiasXAsAcceleration(bx2);
5106 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5107 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5108 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5109 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5110 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5111 final Acceleration by2 = new Acceleration(0.0,
5112 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5113 calibrator.getInitialBiasYAsAcceleration(by2);
5114 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5115 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5116 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5117 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5118 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5119 final Acceleration bz2 = new Acceleration(0.0,
5120 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5121 calibrator.getInitialBiasZAsAcceleration(bz2);
5122 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5123 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5124 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5125 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5126 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5127 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5128 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5129 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5130 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5131 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5132 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5133 final double[] bias1 = calibrator.getInitialBias();
5134 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
5135 final double[] bias2 = new double[3];
5136 calibrator.getInitialBias(bias2);
5137 assertArrayEquals(bias1, bias2, 0.0);
5138 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5139 assertEquals(b1, ba);
5140 final Matrix b2 = new Matrix(3, 1);
5141 calibrator.getInitialBiasAsMatrix(b2);
5142 assertEquals(b1, b2);
5143 final Matrix ma1 = new Matrix(3, 3);
5144 ma1.setSubmatrix(0, 0,
5145 2, 2,
5146 new double[]{ sx, myx, mzx,
5147 mxy, sy, mzy,
5148 mxz, myz, sz});
5149 assertEquals(calibrator.getInitialMa(), ma1);
5150 final Matrix ma2 = new Matrix(3, 3);
5151 calibrator.getInitialMa(ma2);
5152 assertEquals(ma1, ma2);
5153 assertNull(calibrator.getEcefPosition());
5154 assertNull(calibrator.getNedPosition());
5155 assertFalse(calibrator.getNedPosition(null));
5156 assertNull(calibrator.getMeasurements());
5157 assertFalse(calibrator.isCommonAxisUsed());
5158 assertSame(calibrator.getListener(), this);
5159 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5160 assertFalse(calibrator.isReady());
5161 assertFalse(calibrator.isRunning());
5162 assertNull(calibrator.getEstimatedBiases());
5163 assertFalse(calibrator.getEstimatedBiases(null));
5164 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5165 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5166 assertNull(calibrator.getEstimatedBiasFx());
5167 assertNull(calibrator.getEstimatedBiasFy());
5168 assertNull(calibrator.getEstimatedBiasFz());
5169 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5170 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5171 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5172 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5173 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5174 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5175 assertNull(calibrator.getEstimatedMa());
5176 assertNull(calibrator.getEstimatedSx());
5177 assertNull(calibrator.getEstimatedSy());
5178 assertNull(calibrator.getEstimatedSz());
5179 assertNull(calibrator.getEstimatedMxy());
5180 assertNull(calibrator.getEstimatedMxz());
5181 assertNull(calibrator.getEstimatedMyx());
5182 assertNull(calibrator.getEstimatedMyz());
5183 assertNull(calibrator.getEstimatedMzx());
5184 assertNull(calibrator.getEstimatedMzy());
5185 assertNull(calibrator.getEstimatedCovariance());
5186 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5187 }
5188
5189 @Test
5190 public void testConstructor49() throws WrongSizeException {
5191
5192 final Collection<StandardDeviationBodyKinematics> measurements =
5193 Collections.emptyList();
5194
5195 final Matrix ba = generateBa();
5196 final double biasX = ba.getElementAtIndex(0);
5197 final double biasY = ba.getElementAtIndex(1);
5198 final double biasZ = ba.getElementAtIndex(2);
5199
5200 final Matrix ma = generateMaCommonAxis();
5201 final double sx = ma.getElementAt(0, 0);
5202 final double sy = ma.getElementAt(1, 1);
5203 final double sz = ma.getElementAt(2, 2);
5204 final double mxy = ma.getElementAt(0, 1);
5205 final double mxz = ma.getElementAt(0, 2);
5206 final double myx = ma.getElementAt(1, 0);
5207 final double myz = ma.getElementAt(1, 2);
5208 final double mzx = ma.getElementAt(2, 0);
5209 final double mzy = ma.getElementAt(2, 1);
5210
5211 final Acceleration bx = new Acceleration(biasX,
5212 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5213 final Acceleration by = new Acceleration(biasY,
5214 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5215 final Acceleration bz = new Acceleration(biasZ,
5216 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5217
5218 final KnownPositionAccelerometerCalibrator calibrator =
5219 new KnownPositionAccelerometerCalibrator(measurements, bx, by, bz,
5220 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
5221
5222
5223 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5224 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5225 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5226 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5227 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5228 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5229 final Acceleration bx2 = new Acceleration(0.0,
5230 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5231 calibrator.getInitialBiasXAsAcceleration(bx2);
5232 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5233 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5234 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5235 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5236 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5237 final Acceleration by2 = new Acceleration(0.0,
5238 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5239 calibrator.getInitialBiasYAsAcceleration(by2);
5240 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5241 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5242 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5243 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5244 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5245 final Acceleration bz2 = new Acceleration(0.0,
5246 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5247 calibrator.getInitialBiasZAsAcceleration(bz2);
5248 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5249 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5250 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5251 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5252 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5253 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5254 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5255 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5256 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5257 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5258 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5259 final double[] bias1 = calibrator.getInitialBias();
5260 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
5261 final double[] bias2 = new double[3];
5262 calibrator.getInitialBias(bias2);
5263 assertArrayEquals(bias1, bias2, 0.0);
5264 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5265 assertEquals(b1, ba);
5266 final Matrix b2 = new Matrix(3, 1);
5267 calibrator.getInitialBiasAsMatrix(b2);
5268 assertEquals(b1, b2);
5269 final Matrix ma1 = new Matrix(3, 3);
5270 ma1.setSubmatrix(0, 0,
5271 2, 2,
5272 new double[]{ sx, myx, mzx,
5273 mxy, sy, mzy,
5274 mxz, myz, sz});
5275 assertEquals(calibrator.getInitialMa(), ma1);
5276 final Matrix ma2 = new Matrix(3, 3);
5277 calibrator.getInitialMa(ma2);
5278 assertEquals(ma1, ma2);
5279 assertNull(calibrator.getEcefPosition());
5280 assertNull(calibrator.getNedPosition());
5281 assertFalse(calibrator.getNedPosition(null));
5282 assertSame(calibrator.getMeasurements(), measurements);
5283 assertFalse(calibrator.isCommonAxisUsed());
5284 assertNull(calibrator.getListener());
5285 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5286 assertFalse(calibrator.isReady());
5287 assertFalse(calibrator.isRunning());
5288 assertNull(calibrator.getEstimatedBiases());
5289 assertFalse(calibrator.getEstimatedBiases(null));
5290 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5291 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5292 assertNull(calibrator.getEstimatedBiasFx());
5293 assertNull(calibrator.getEstimatedBiasFy());
5294 assertNull(calibrator.getEstimatedBiasFz());
5295 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5296 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5297 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5298 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5299 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5300 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5301 assertNull(calibrator.getEstimatedMa());
5302 assertNull(calibrator.getEstimatedSx());
5303 assertNull(calibrator.getEstimatedSy());
5304 assertNull(calibrator.getEstimatedSz());
5305 assertNull(calibrator.getEstimatedMxy());
5306 assertNull(calibrator.getEstimatedMxz());
5307 assertNull(calibrator.getEstimatedMyx());
5308 assertNull(calibrator.getEstimatedMyz());
5309 assertNull(calibrator.getEstimatedMzx());
5310 assertNull(calibrator.getEstimatedMzy());
5311 assertNull(calibrator.getEstimatedCovariance());
5312 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5313 }
5314
5315 @Test
5316 public void testConstructor50() throws WrongSizeException {
5317
5318 final Collection<StandardDeviationBodyKinematics> measurements =
5319 Collections.emptyList();
5320
5321 final Matrix ba = generateBa();
5322 final double biasX = ba.getElementAtIndex(0);
5323 final double biasY = ba.getElementAtIndex(1);
5324 final double biasZ = ba.getElementAtIndex(2);
5325
5326 final Matrix ma = generateMaCommonAxis();
5327 final double sx = ma.getElementAt(0, 0);
5328 final double sy = ma.getElementAt(1, 1);
5329 final double sz = ma.getElementAt(2, 2);
5330 final double mxy = ma.getElementAt(0, 1);
5331 final double mxz = ma.getElementAt(0, 2);
5332 final double myx = ma.getElementAt(1, 0);
5333 final double myz = ma.getElementAt(1, 2);
5334 final double mzx = ma.getElementAt(2, 0);
5335 final double mzy = ma.getElementAt(2, 1);
5336
5337 final Acceleration bx = new Acceleration(biasX,
5338 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5339 final Acceleration by = new Acceleration(biasY,
5340 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5341 final Acceleration bz = new Acceleration(biasZ,
5342 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5343
5344 final KnownPositionAccelerometerCalibrator calibrator =
5345 new KnownPositionAccelerometerCalibrator(measurements, bx, by, bz,
5346 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
5347
5348
5349 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5350 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5351 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5352 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5353 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5354 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5355 final Acceleration bx2 = new Acceleration(0.0,
5356 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5357 calibrator.getInitialBiasXAsAcceleration(bx2);
5358 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5359 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5360 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5361 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5362 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5363 final Acceleration by2 = new Acceleration(0.0,
5364 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5365 calibrator.getInitialBiasYAsAcceleration(by2);
5366 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5367 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5368 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5369 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5370 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5371 final Acceleration bz2 = new Acceleration(0.0,
5372 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5373 calibrator.getInitialBiasZAsAcceleration(bz2);
5374 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5375 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5376 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5377 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5378 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5379 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5380 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5381 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5382 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5383 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5384 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5385 final double[] bias1 = calibrator.getInitialBias();
5386 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
5387 final double[] bias2 = new double[3];
5388 calibrator.getInitialBias(bias2);
5389 assertArrayEquals(bias1, bias2, 0.0);
5390 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5391 assertEquals(b1, ba);
5392 final Matrix b2 = new Matrix(3, 1);
5393 calibrator.getInitialBiasAsMatrix(b2);
5394 assertEquals(b1, b2);
5395 final Matrix ma1 = new Matrix(3, 3);
5396 ma1.setSubmatrix(0, 0,
5397 2, 2,
5398 new double[]{ sx, myx, mzx,
5399 mxy, sy, mzy,
5400 mxz, myz, sz});
5401 assertEquals(calibrator.getInitialMa(), ma1);
5402 final Matrix ma2 = new Matrix(3, 3);
5403 calibrator.getInitialMa(ma2);
5404 assertEquals(ma1, ma2);
5405 assertNull(calibrator.getEcefPosition());
5406 assertNull(calibrator.getNedPosition());
5407 assertFalse(calibrator.getNedPosition(null));
5408 assertSame(calibrator.getMeasurements(), measurements);
5409 assertFalse(calibrator.isCommonAxisUsed());
5410 assertSame(calibrator.getListener(), this);
5411 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5412 assertFalse(calibrator.isReady());
5413 assertFalse(calibrator.isRunning());
5414 assertNull(calibrator.getEstimatedBiases());
5415 assertFalse(calibrator.getEstimatedBiases(null));
5416 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5417 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5418 assertNull(calibrator.getEstimatedBiasFx());
5419 assertNull(calibrator.getEstimatedBiasFy());
5420 assertNull(calibrator.getEstimatedBiasFz());
5421 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5422 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5423 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5424 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5425 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5426 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5427 assertNull(calibrator.getEstimatedMa());
5428 assertNull(calibrator.getEstimatedSx());
5429 assertNull(calibrator.getEstimatedSy());
5430 assertNull(calibrator.getEstimatedSz());
5431 assertNull(calibrator.getEstimatedMxy());
5432 assertNull(calibrator.getEstimatedMxz());
5433 assertNull(calibrator.getEstimatedMyx());
5434 assertNull(calibrator.getEstimatedMyz());
5435 assertNull(calibrator.getEstimatedMzx());
5436 assertNull(calibrator.getEstimatedMzy());
5437 assertNull(calibrator.getEstimatedCovariance());
5438 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5439 }
5440
5441 @Test
5442 public void testConstructor51() throws WrongSizeException {
5443 final Matrix ba = generateBa();
5444 final double biasX = ba.getElementAtIndex(0);
5445 final double biasY = ba.getElementAtIndex(1);
5446 final double biasZ = ba.getElementAtIndex(2);
5447
5448 final Matrix ma = generateMaCommonAxis();
5449 final double sx = ma.getElementAt(0, 0);
5450 final double sy = ma.getElementAt(1, 1);
5451 final double sz = ma.getElementAt(2, 2);
5452 final double mxy = ma.getElementAt(0, 1);
5453 final double mxz = ma.getElementAt(0, 2);
5454 final double myx = ma.getElementAt(1, 0);
5455 final double myz = ma.getElementAt(1, 2);
5456 final double mzx = ma.getElementAt(2, 0);
5457 final double mzy = ma.getElementAt(2, 1);
5458
5459 final Acceleration bx = new Acceleration(biasX,
5460 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5461 final Acceleration by = new Acceleration(biasY,
5462 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5463 final Acceleration bz = new Acceleration(biasZ,
5464 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5465
5466 final KnownPositionAccelerometerCalibrator calibrator =
5467 new KnownPositionAccelerometerCalibrator(true,
5468 bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
5469
5470
5471 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5472 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5473 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5474 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5475 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5476 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5477 final Acceleration bx2 = new Acceleration(0.0,
5478 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5479 calibrator.getInitialBiasXAsAcceleration(bx2);
5480 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5481 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5482 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5483 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5484 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5485 final Acceleration by2 = new Acceleration(0.0,
5486 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5487 calibrator.getInitialBiasYAsAcceleration(by2);
5488 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5489 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5490 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5491 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5492 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5493 final Acceleration bz2 = new Acceleration(0.0,
5494 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5495 calibrator.getInitialBiasZAsAcceleration(bz2);
5496 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5497 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5498 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5499 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5500 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5501 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5502 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5503 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5504 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5505 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5506 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5507 final double[] bias1 = calibrator.getInitialBias();
5508 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
5509 final double[] bias2 = new double[3];
5510 calibrator.getInitialBias(bias2);
5511 assertArrayEquals(bias1, bias2, 0.0);
5512 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5513 assertEquals(b1, ba);
5514 final Matrix b2 = new Matrix(3, 1);
5515 calibrator.getInitialBiasAsMatrix(b2);
5516 assertEquals(b1, b2);
5517 final Matrix ma1 = new Matrix(3, 3);
5518 ma1.setSubmatrix(0, 0,
5519 2, 2,
5520 new double[]{ sx, myx, mzx,
5521 mxy, sy, mzy,
5522 mxz, myz, sz});
5523 assertEquals(calibrator.getInitialMa(), ma1);
5524 final Matrix ma2 = new Matrix(3, 3);
5525 calibrator.getInitialMa(ma2);
5526 assertEquals(ma1, ma2);
5527 assertNull(calibrator.getEcefPosition());
5528 assertNull(calibrator.getNedPosition());
5529 assertFalse(calibrator.getNedPosition(null));
5530 assertNull(calibrator.getMeasurements());
5531 assertTrue(calibrator.isCommonAxisUsed());
5532 assertNull(calibrator.getListener());
5533 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5534 assertFalse(calibrator.isReady());
5535 assertFalse(calibrator.isRunning());
5536 assertNull(calibrator.getEstimatedBiases());
5537 assertFalse(calibrator.getEstimatedBiases(null));
5538 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5539 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5540 assertNull(calibrator.getEstimatedBiasFx());
5541 assertNull(calibrator.getEstimatedBiasFy());
5542 assertNull(calibrator.getEstimatedBiasFz());
5543 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5544 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5545 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5546 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5547 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5548 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5549 assertNull(calibrator.getEstimatedMa());
5550 assertNull(calibrator.getEstimatedSx());
5551 assertNull(calibrator.getEstimatedSy());
5552 assertNull(calibrator.getEstimatedSz());
5553 assertNull(calibrator.getEstimatedMxy());
5554 assertNull(calibrator.getEstimatedMxz());
5555 assertNull(calibrator.getEstimatedMyx());
5556 assertNull(calibrator.getEstimatedMyz());
5557 assertNull(calibrator.getEstimatedMzx());
5558 assertNull(calibrator.getEstimatedMzy());
5559 assertNull(calibrator.getEstimatedCovariance());
5560 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5561 }
5562
5563 @Test
5564 public void testConstructor52() throws WrongSizeException {
5565 final Matrix ba = generateBa();
5566 final double biasX = ba.getElementAtIndex(0);
5567 final double biasY = ba.getElementAtIndex(1);
5568 final double biasZ = ba.getElementAtIndex(2);
5569
5570 final Matrix ma = generateMaCommonAxis();
5571 final double sx = ma.getElementAt(0, 0);
5572 final double sy = ma.getElementAt(1, 1);
5573 final double sz = ma.getElementAt(2, 2);
5574 final double mxy = ma.getElementAt(0, 1);
5575 final double mxz = ma.getElementAt(0, 2);
5576 final double myx = ma.getElementAt(1, 0);
5577 final double myz = ma.getElementAt(1, 2);
5578 final double mzx = ma.getElementAt(2, 0);
5579 final double mzy = ma.getElementAt(2, 1);
5580
5581 final Acceleration bx = new Acceleration(biasX,
5582 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5583 final Acceleration by = new Acceleration(biasY,
5584 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5585 final Acceleration bz = new Acceleration(biasZ,
5586 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5587
5588 final KnownPositionAccelerometerCalibrator calibrator =
5589 new KnownPositionAccelerometerCalibrator(true,
5590 bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
5591 this);
5592
5593
5594 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5595 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5596 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5597 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5598 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5599 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5600 final Acceleration bx2 = new Acceleration(0.0,
5601 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5602 calibrator.getInitialBiasXAsAcceleration(bx2);
5603 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5604 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5605 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5606 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5607 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5608 final Acceleration by2 = new Acceleration(0.0,
5609 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5610 calibrator.getInitialBiasYAsAcceleration(by2);
5611 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5612 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5613 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5614 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5615 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5616 final Acceleration bz2 = new Acceleration(0.0,
5617 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5618 calibrator.getInitialBiasZAsAcceleration(bz2);
5619 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5620 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5621 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5622 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5623 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5624 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5625 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5626 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5627 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5628 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5629 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5630 final double[] bias1 = calibrator.getInitialBias();
5631 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
5632 final double[] bias2 = new double[3];
5633 calibrator.getInitialBias(bias2);
5634 assertArrayEquals(bias1, bias2, 0.0);
5635 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5636 assertEquals(b1, ba);
5637 final Matrix b2 = new Matrix(3, 1);
5638 calibrator.getInitialBiasAsMatrix(b2);
5639 assertEquals(b1, b2);
5640 final Matrix ma1 = new Matrix(3, 3);
5641 ma1.setSubmatrix(0, 0,
5642 2, 2,
5643 new double[]{ sx, myx, mzx,
5644 mxy, sy, mzy,
5645 mxz, myz, sz});
5646 assertEquals(calibrator.getInitialMa(), ma1);
5647 final Matrix ma2 = new Matrix(3, 3);
5648 calibrator.getInitialMa(ma2);
5649 assertEquals(ma1, ma2);
5650 assertNull(calibrator.getEcefPosition());
5651 assertNull(calibrator.getNedPosition());
5652 assertFalse(calibrator.getNedPosition(null));
5653 assertNull(calibrator.getMeasurements());
5654 assertTrue(calibrator.isCommonAxisUsed());
5655 assertSame(calibrator.getListener(), this);
5656 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5657 assertFalse(calibrator.isReady());
5658 assertFalse(calibrator.isRunning());
5659 assertNull(calibrator.getEstimatedBiases());
5660 assertFalse(calibrator.getEstimatedBiases(null));
5661 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5662 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5663 assertNull(calibrator.getEstimatedBiasFx());
5664 assertNull(calibrator.getEstimatedBiasFy());
5665 assertNull(calibrator.getEstimatedBiasFz());
5666 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5667 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5668 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5669 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5670 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5671 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5672 assertNull(calibrator.getEstimatedMa());
5673 assertNull(calibrator.getEstimatedSx());
5674 assertNull(calibrator.getEstimatedSy());
5675 assertNull(calibrator.getEstimatedSz());
5676 assertNull(calibrator.getEstimatedMxy());
5677 assertNull(calibrator.getEstimatedMxz());
5678 assertNull(calibrator.getEstimatedMyx());
5679 assertNull(calibrator.getEstimatedMyz());
5680 assertNull(calibrator.getEstimatedMzx());
5681 assertNull(calibrator.getEstimatedMzy());
5682 assertNull(calibrator.getEstimatedCovariance());
5683 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5684 }
5685
5686 @Test
5687 public void testConstructor53() throws WrongSizeException {
5688 final Collection<StandardDeviationBodyKinematics> measurements =
5689 Collections.emptyList();
5690
5691 final Matrix ba = generateBa();
5692 final double biasX = ba.getElementAtIndex(0);
5693 final double biasY = ba.getElementAtIndex(1);
5694 final double biasZ = ba.getElementAtIndex(2);
5695
5696 final Matrix ma = generateMaCommonAxis();
5697 final double sx = ma.getElementAt(0, 0);
5698 final double sy = ma.getElementAt(1, 1);
5699 final double sz = ma.getElementAt(2, 2);
5700 final double mxy = ma.getElementAt(0, 1);
5701 final double mxz = ma.getElementAt(0, 2);
5702 final double myx = ma.getElementAt(1, 0);
5703 final double myz = ma.getElementAt(1, 2);
5704 final double mzx = ma.getElementAt(2, 0);
5705 final double mzy = ma.getElementAt(2, 1);
5706
5707 final Acceleration bx = new Acceleration(biasX,
5708 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5709 final Acceleration by = new Acceleration(biasY,
5710 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5711 final Acceleration bz = new Acceleration(biasZ,
5712 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5713
5714 final KnownPositionAccelerometerCalibrator calibrator =
5715 new KnownPositionAccelerometerCalibrator(measurements,
5716 true, bx, by, bz, sx, sy, sz,
5717 mxy, mxz, myx, myz, mzx, mzy);
5718
5719
5720 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5721 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5722 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5723 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5724 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5725 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5726 final Acceleration bx2 = new Acceleration(0.0,
5727 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5728 calibrator.getInitialBiasXAsAcceleration(bx2);
5729 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5730 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5731 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5732 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5733 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5734 final Acceleration by2 = new Acceleration(0.0,
5735 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5736 calibrator.getInitialBiasYAsAcceleration(by2);
5737 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5738 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5739 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5740 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5741 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5742 final Acceleration bz2 = new Acceleration(0.0,
5743 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5744 calibrator.getInitialBiasZAsAcceleration(bz2);
5745 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5746 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5747 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5748 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5749 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5750 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5751 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5752 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5753 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5754 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5755 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5756 final double[] bias1 = calibrator.getInitialBias();
5757 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
5758 final double[] bias2 = new double[3];
5759 calibrator.getInitialBias(bias2);
5760 assertArrayEquals(bias1, bias2, 0.0);
5761 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5762 assertEquals(b1, ba);
5763 final Matrix b2 = new Matrix(3, 1);
5764 calibrator.getInitialBiasAsMatrix(b2);
5765 assertEquals(b1, b2);
5766 final Matrix ma1 = new Matrix(3, 3);
5767 ma1.setSubmatrix(0, 0,
5768 2, 2,
5769 new double[]{ sx, myx, mzx,
5770 mxy, sy, mzy,
5771 mxz, myz, sz});
5772 assertEquals(calibrator.getInitialMa(), ma1);
5773 final Matrix ma2 = new Matrix(3, 3);
5774 calibrator.getInitialMa(ma2);
5775 assertEquals(ma1, ma2);
5776 assertNull(calibrator.getEcefPosition());
5777 assertNull(calibrator.getNedPosition());
5778 assertFalse(calibrator.getNedPosition(null));
5779 assertSame(calibrator.getMeasurements(), measurements);
5780 assertTrue(calibrator.isCommonAxisUsed());
5781 assertNull(calibrator.getListener());
5782 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5783 assertFalse(calibrator.isReady());
5784 assertFalse(calibrator.isRunning());
5785 assertNull(calibrator.getEstimatedBiases());
5786 assertFalse(calibrator.getEstimatedBiases(null));
5787 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5788 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5789 assertNull(calibrator.getEstimatedBiasFx());
5790 assertNull(calibrator.getEstimatedBiasFy());
5791 assertNull(calibrator.getEstimatedBiasFz());
5792 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5793 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5794 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5795 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5796 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5797 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5798 assertNull(calibrator.getEstimatedMa());
5799 assertNull(calibrator.getEstimatedSx());
5800 assertNull(calibrator.getEstimatedSy());
5801 assertNull(calibrator.getEstimatedSz());
5802 assertNull(calibrator.getEstimatedMxy());
5803 assertNull(calibrator.getEstimatedMxz());
5804 assertNull(calibrator.getEstimatedMyx());
5805 assertNull(calibrator.getEstimatedMyz());
5806 assertNull(calibrator.getEstimatedMzx());
5807 assertNull(calibrator.getEstimatedMzy());
5808 assertNull(calibrator.getEstimatedCovariance());
5809 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5810 }
5811
5812 @Test
5813 public void testConstructor54() throws WrongSizeException {
5814 final Collection<StandardDeviationBodyKinematics> measurements =
5815 Collections.emptyList();
5816
5817 final Matrix ba = generateBa();
5818 final double biasX = ba.getElementAtIndex(0);
5819 final double biasY = ba.getElementAtIndex(1);
5820 final double biasZ = ba.getElementAtIndex(2);
5821
5822 final Matrix ma = generateMaCommonAxis();
5823 final double sx = ma.getElementAt(0, 0);
5824 final double sy = ma.getElementAt(1, 1);
5825 final double sz = ma.getElementAt(2, 2);
5826 final double mxy = ma.getElementAt(0, 1);
5827 final double mxz = ma.getElementAt(0, 2);
5828 final double myx = ma.getElementAt(1, 0);
5829 final double myz = ma.getElementAt(1, 2);
5830 final double mzx = ma.getElementAt(2, 0);
5831 final double mzy = ma.getElementAt(2, 1);
5832
5833 final Acceleration bx = new Acceleration(biasX,
5834 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5835 final Acceleration by = new Acceleration(biasY,
5836 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5837 final Acceleration bz = new Acceleration(biasZ,
5838 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5839
5840 final KnownPositionAccelerometerCalibrator calibrator =
5841 new KnownPositionAccelerometerCalibrator(measurements,
5842 true, bx, by, bz, sx, sy, sz,
5843 mxy, mxz, myx, myz, mzx, mzy, this);
5844
5845
5846 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5847 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5848 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5849 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5850 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5851 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5852 final Acceleration bx2 = new Acceleration(0.0,
5853 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5854 calibrator.getInitialBiasXAsAcceleration(bx2);
5855 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5856 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5857 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5858 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5859 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5860 final Acceleration by2 = new Acceleration(0.0,
5861 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5862 calibrator.getInitialBiasYAsAcceleration(by2);
5863 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5864 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5865 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5866 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5867 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5868 final Acceleration bz2 = new Acceleration(0.0,
5869 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5870 calibrator.getInitialBiasZAsAcceleration(bz2);
5871 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5872 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5873 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5874 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5875 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5876 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5877 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5878 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5879 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5880 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5881 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5882 final double[] bias1 = calibrator.getInitialBias();
5883 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
5884 final double[] bias2 = new double[3];
5885 calibrator.getInitialBias(bias2);
5886 assertArrayEquals(bias1, bias2, 0.0);
5887 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5888 assertEquals(b1, ba);
5889 final Matrix b2 = new Matrix(3, 1);
5890 calibrator.getInitialBiasAsMatrix(b2);
5891 assertEquals(b1, b2);
5892 final Matrix ma1 = new Matrix(3, 3);
5893 ma1.setSubmatrix(0, 0,
5894 2, 2,
5895 new double[]{ sx, myx, mzx,
5896 mxy, sy, mzy,
5897 mxz, myz, sz});
5898 assertEquals(calibrator.getInitialMa(), ma1);
5899 final Matrix ma2 = new Matrix(3, 3);
5900 calibrator.getInitialMa(ma2);
5901 assertEquals(ma1, ma2);
5902 assertNull(calibrator.getEcefPosition());
5903 assertNull(calibrator.getNedPosition());
5904 assertFalse(calibrator.getNedPosition(null));
5905 assertSame(calibrator.getMeasurements(), measurements);
5906 assertTrue(calibrator.isCommonAxisUsed());
5907 assertSame(calibrator.getListener(), this);
5908 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5909 assertFalse(calibrator.isReady());
5910 assertFalse(calibrator.isRunning());
5911 assertNull(calibrator.getEstimatedBiases());
5912 assertFalse(calibrator.getEstimatedBiases(null));
5913 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5914 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5915 assertNull(calibrator.getEstimatedBiasFx());
5916 assertNull(calibrator.getEstimatedBiasFy());
5917 assertNull(calibrator.getEstimatedBiasFz());
5918 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5919 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5920 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5921 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5922 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5923 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5924 assertNull(calibrator.getEstimatedMa());
5925 assertNull(calibrator.getEstimatedSx());
5926 assertNull(calibrator.getEstimatedSy());
5927 assertNull(calibrator.getEstimatedSz());
5928 assertNull(calibrator.getEstimatedMxy());
5929 assertNull(calibrator.getEstimatedMxz());
5930 assertNull(calibrator.getEstimatedMyx());
5931 assertNull(calibrator.getEstimatedMyz());
5932 assertNull(calibrator.getEstimatedMzx());
5933 assertNull(calibrator.getEstimatedMzy());
5934 assertNull(calibrator.getEstimatedCovariance());
5935 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5936 }
5937
5938 @Test
5939 public void testConstructor55() throws WrongSizeException {
5940 final Matrix ba = generateBa();
5941 final double[] bias = ba.getBuffer();
5942 final double biasX = ba.getElementAtIndex(0);
5943 final double biasY = ba.getElementAtIndex(1);
5944 final double biasZ = ba.getElementAtIndex(2);
5945
5946 KnownPositionAccelerometerCalibrator calibrator =
5947 new KnownPositionAccelerometerCalibrator(bias);
5948
5949
5950 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5951 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5952 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5953 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5954 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5955 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5956 final Acceleration bx2 = new Acceleration(0.0,
5957 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5958 calibrator.getInitialBiasXAsAcceleration(bx2);
5959 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5960 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5961 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5962 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5963 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5964 final Acceleration by2 = new Acceleration(0.0,
5965 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5966 calibrator.getInitialBiasYAsAcceleration(by2);
5967 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5968 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5969 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5970 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5971 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5972 final Acceleration bz2 = new Acceleration(0.0,
5973 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5974 calibrator.getInitialBiasZAsAcceleration(bz2);
5975 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5976 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5977 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
5978 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
5979 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
5980 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
5981 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
5982 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
5983 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
5984 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
5985 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
5986 final double[] bias1 = calibrator.getInitialBias();
5987 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
5988 final double[] bias2 = new double[3];
5989 calibrator.getInitialBias(bias2);
5990 assertArrayEquals(bias1, bias2, 0.0);
5991 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5992 assertEquals(b1, ba);
5993 final Matrix b2 = new Matrix(3, 1);
5994 calibrator.getInitialBiasAsMatrix(b2);
5995 assertEquals(b1, b2);
5996 final Matrix ma1 = calibrator.getInitialMa();
5997 assertEquals(ma1, new Matrix(3, 3));
5998 final Matrix ma2 = new Matrix(3, 3);
5999 calibrator.getInitialMa(ma2);
6000 assertEquals(ma1, ma2);
6001 assertNull(calibrator.getEcefPosition());
6002 assertNull(calibrator.getNedPosition());
6003 assertFalse(calibrator.getNedPosition(null));
6004 assertNull(calibrator.getMeasurements());
6005 assertFalse(calibrator.isCommonAxisUsed());
6006 assertNull(calibrator.getListener());
6007 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6008 assertFalse(calibrator.isReady());
6009 assertFalse(calibrator.isRunning());
6010 assertNull(calibrator.getEstimatedBiases());
6011 assertFalse(calibrator.getEstimatedBiases(null));
6012 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6013 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6014 assertNull(calibrator.getEstimatedBiasFx());
6015 assertNull(calibrator.getEstimatedBiasFy());
6016 assertNull(calibrator.getEstimatedBiasFz());
6017 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6018 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6019 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6020 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6021 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6022 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6023 assertNull(calibrator.getEstimatedMa());
6024 assertNull(calibrator.getEstimatedSx());
6025 assertNull(calibrator.getEstimatedSy());
6026 assertNull(calibrator.getEstimatedSz());
6027 assertNull(calibrator.getEstimatedMxy());
6028 assertNull(calibrator.getEstimatedMxz());
6029 assertNull(calibrator.getEstimatedMyx());
6030 assertNull(calibrator.getEstimatedMyz());
6031 assertNull(calibrator.getEstimatedMzx());
6032 assertNull(calibrator.getEstimatedMzy());
6033 assertNull(calibrator.getEstimatedCovariance());
6034 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6035
6036
6037 calibrator = null;
6038 try {
6039 calibrator = new KnownPositionAccelerometerCalibrator(new double[1]);
6040 fail("IllegalArgumentException expected but not thrown");
6041 } catch (final IllegalArgumentException ignore) {
6042 }
6043 assertNull(calibrator);
6044 }
6045
6046 @Test
6047 public void testConstructor56() throws WrongSizeException {
6048 final Matrix ba = generateBa();
6049 final double[] bias = ba.getBuffer();
6050 final double biasX = ba.getElementAtIndex(0);
6051 final double biasY = ba.getElementAtIndex(1);
6052 final double biasZ = ba.getElementAtIndex(2);
6053
6054 KnownPositionAccelerometerCalibrator calibrator =
6055 new KnownPositionAccelerometerCalibrator(bias, this);
6056
6057
6058 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6059 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6060 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6061 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6062 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6063 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6064 final Acceleration bx2 = new Acceleration(0.0,
6065 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6066 calibrator.getInitialBiasXAsAcceleration(bx2);
6067 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6068 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6069 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6070 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6071 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6072 final Acceleration by2 = new Acceleration(0.0,
6073 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6074 calibrator.getInitialBiasYAsAcceleration(by2);
6075 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6076 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6077 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6078 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6079 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6080 final Acceleration bz2 = new Acceleration(0.0,
6081 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6082 calibrator.getInitialBiasZAsAcceleration(bz2);
6083 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6084 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6085 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6086 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6087 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6088 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6089 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6090 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6091 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6092 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6093 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6094 final double[] bias1 = calibrator.getInitialBias();
6095 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
6096 final double[] bias2 = new double[3];
6097 calibrator.getInitialBias(bias2);
6098 assertArrayEquals(bias1, bias2, 0.0);
6099 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6100 assertEquals(b1, ba);
6101 final Matrix b2 = new Matrix(3, 1);
6102 calibrator.getInitialBiasAsMatrix(b2);
6103 assertEquals(b1, b2);
6104 final Matrix ma1 = calibrator.getInitialMa();
6105 assertEquals(ma1, new Matrix(3, 3));
6106 final Matrix ma2 = new Matrix(3, 3);
6107 calibrator.getInitialMa(ma2);
6108 assertEquals(ma1, ma2);
6109 assertNull(calibrator.getEcefPosition());
6110 assertNull(calibrator.getNedPosition());
6111 assertFalse(calibrator.getNedPosition(null));
6112 assertNull(calibrator.getMeasurements());
6113 assertFalse(calibrator.isCommonAxisUsed());
6114 assertSame(calibrator.getListener(), this);
6115 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6116 assertFalse(calibrator.isReady());
6117 assertFalse(calibrator.isRunning());
6118 assertNull(calibrator.getEstimatedBiases());
6119 assertFalse(calibrator.getEstimatedBiases(null));
6120 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6121 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6122 assertNull(calibrator.getEstimatedBiasFx());
6123 assertNull(calibrator.getEstimatedBiasFy());
6124 assertNull(calibrator.getEstimatedBiasFz());
6125 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6126 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6127 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6128 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6129 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6130 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6131 assertNull(calibrator.getEstimatedMa());
6132 assertNull(calibrator.getEstimatedSx());
6133 assertNull(calibrator.getEstimatedSy());
6134 assertNull(calibrator.getEstimatedSz());
6135 assertNull(calibrator.getEstimatedMxy());
6136 assertNull(calibrator.getEstimatedMxz());
6137 assertNull(calibrator.getEstimatedMyx());
6138 assertNull(calibrator.getEstimatedMyz());
6139 assertNull(calibrator.getEstimatedMzx());
6140 assertNull(calibrator.getEstimatedMzy());
6141 assertNull(calibrator.getEstimatedCovariance());
6142 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6143
6144
6145 calibrator = null;
6146 try {
6147 calibrator = new KnownPositionAccelerometerCalibrator(new double[1],
6148 this);
6149 fail("IllegalArgumentException expected but not thrown");
6150 } catch (final IllegalArgumentException ignore) {
6151 }
6152 assertNull(calibrator);
6153 }
6154
6155 @Test
6156 public void testConstructor57() throws WrongSizeException {
6157 final Collection<StandardDeviationBodyKinematics> measurements =
6158 Collections.emptyList();
6159
6160 final Matrix ba = generateBa();
6161 final double[] bias = ba.getBuffer();
6162 final double biasX = ba.getElementAtIndex(0);
6163 final double biasY = ba.getElementAtIndex(1);
6164 final double biasZ = ba.getElementAtIndex(2);
6165
6166 KnownPositionAccelerometerCalibrator calibrator =
6167 new KnownPositionAccelerometerCalibrator(measurements, bias);
6168
6169
6170 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6171 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6172 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6173 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6174 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6175 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6176 final Acceleration bx2 = new Acceleration(0.0,
6177 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6178 calibrator.getInitialBiasXAsAcceleration(bx2);
6179 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6180 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6181 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6182 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6183 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6184 final Acceleration by2 = new Acceleration(0.0,
6185 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6186 calibrator.getInitialBiasYAsAcceleration(by2);
6187 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6188 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6189 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6190 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6191 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6192 final Acceleration bz2 = new Acceleration(0.0,
6193 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6194 calibrator.getInitialBiasZAsAcceleration(bz2);
6195 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6196 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6197 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6198 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6199 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6200 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6201 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6202 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6203 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6204 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6205 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6206 final double[] bias1 = calibrator.getInitialBias();
6207 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
6208 final double[] bias2 = new double[3];
6209 calibrator.getInitialBias(bias2);
6210 assertArrayEquals(bias1, bias2, 0.0);
6211 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6212 assertEquals(b1, ba);
6213 final Matrix b2 = new Matrix(3, 1);
6214 calibrator.getInitialBiasAsMatrix(b2);
6215 assertEquals(b1, b2);
6216 final Matrix ma1 = calibrator.getInitialMa();
6217 assertEquals(ma1, new Matrix(3, 3));
6218 final Matrix ma2 = new Matrix(3, 3);
6219 calibrator.getInitialMa(ma2);
6220 assertEquals(ma1, ma2);
6221 assertNull(calibrator.getEcefPosition());
6222 assertNull(calibrator.getNedPosition());
6223 assertFalse(calibrator.getNedPosition(null));
6224 assertSame(calibrator.getMeasurements(), measurements);
6225 assertFalse(calibrator.isCommonAxisUsed());
6226 assertNull(calibrator.getListener());
6227 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6228 assertFalse(calibrator.isReady());
6229 assertFalse(calibrator.isRunning());
6230 assertNull(calibrator.getEstimatedBiases());
6231 assertFalse(calibrator.getEstimatedBiases(null));
6232 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6233 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6234 assertNull(calibrator.getEstimatedBiasFx());
6235 assertNull(calibrator.getEstimatedBiasFy());
6236 assertNull(calibrator.getEstimatedBiasFz());
6237 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6238 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6239 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6240 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6241 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6242 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6243 assertNull(calibrator.getEstimatedMa());
6244 assertNull(calibrator.getEstimatedSx());
6245 assertNull(calibrator.getEstimatedSy());
6246 assertNull(calibrator.getEstimatedSz());
6247 assertNull(calibrator.getEstimatedMxy());
6248 assertNull(calibrator.getEstimatedMxz());
6249 assertNull(calibrator.getEstimatedMyx());
6250 assertNull(calibrator.getEstimatedMyz());
6251 assertNull(calibrator.getEstimatedMzx());
6252 assertNull(calibrator.getEstimatedMzy());
6253 assertNull(calibrator.getEstimatedCovariance());
6254 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6255
6256
6257 calibrator = null;
6258 try {
6259 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
6260 new double[1]);
6261 fail("IllegalArgumentException expected but not thrown");
6262 } catch (final IllegalArgumentException ignore) {
6263 }
6264 assertNull(calibrator);
6265 }
6266
6267 @Test
6268 public void testConstructor58() throws WrongSizeException {
6269 final Collection<StandardDeviationBodyKinematics> measurements =
6270 Collections.emptyList();
6271
6272 final Matrix ba = generateBa();
6273 final double[] bias = ba.getBuffer();
6274 final double biasX = ba.getElementAtIndex(0);
6275 final double biasY = ba.getElementAtIndex(1);
6276 final double biasZ = ba.getElementAtIndex(2);
6277
6278 KnownPositionAccelerometerCalibrator calibrator =
6279 new KnownPositionAccelerometerCalibrator(measurements, bias,
6280 this);
6281
6282
6283 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6284 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6285 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6286 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6287 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6288 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6289 final Acceleration bx2 = new Acceleration(0.0,
6290 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6291 calibrator.getInitialBiasXAsAcceleration(bx2);
6292 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6293 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6294 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6295 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6296 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6297 final Acceleration by2 = new Acceleration(0.0,
6298 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6299 calibrator.getInitialBiasYAsAcceleration(by2);
6300 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6301 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6302 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6303 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6304 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6305 final Acceleration bz2 = new Acceleration(0.0,
6306 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6307 calibrator.getInitialBiasZAsAcceleration(bz2);
6308 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6309 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6310 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6311 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6312 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6313 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6314 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6315 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6316 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6317 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6318 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6319 final double[] bias1 = calibrator.getInitialBias();
6320 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
6321 final double[] bias2 = new double[3];
6322 calibrator.getInitialBias(bias2);
6323 assertArrayEquals(bias1, bias2, 0.0);
6324 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6325 assertEquals(b1, ba);
6326 final Matrix b2 = new Matrix(3, 1);
6327 calibrator.getInitialBiasAsMatrix(b2);
6328 assertEquals(b1, b2);
6329 final Matrix ma1 = calibrator.getInitialMa();
6330 assertEquals(ma1, new Matrix(3, 3));
6331 final Matrix ma2 = new Matrix(3, 3);
6332 calibrator.getInitialMa(ma2);
6333 assertEquals(ma1, ma2);
6334 assertNull(calibrator.getEcefPosition());
6335 assertNull(calibrator.getNedPosition());
6336 assertFalse(calibrator.getNedPosition(null));
6337 assertSame(calibrator.getMeasurements(), measurements);
6338 assertFalse(calibrator.isCommonAxisUsed());
6339 assertSame(calibrator.getListener(), this);
6340 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6341 assertFalse(calibrator.isReady());
6342 assertFalse(calibrator.isRunning());
6343 assertNull(calibrator.getEstimatedBiases());
6344 assertFalse(calibrator.getEstimatedBiases(null));
6345 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6346 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6347 assertNull(calibrator.getEstimatedBiasFx());
6348 assertNull(calibrator.getEstimatedBiasFy());
6349 assertNull(calibrator.getEstimatedBiasFz());
6350 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6351 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6352 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6353 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6354 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6355 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6356 assertNull(calibrator.getEstimatedMa());
6357 assertNull(calibrator.getEstimatedSx());
6358 assertNull(calibrator.getEstimatedSy());
6359 assertNull(calibrator.getEstimatedSz());
6360 assertNull(calibrator.getEstimatedMxy());
6361 assertNull(calibrator.getEstimatedMxz());
6362 assertNull(calibrator.getEstimatedMyx());
6363 assertNull(calibrator.getEstimatedMyz());
6364 assertNull(calibrator.getEstimatedMzx());
6365 assertNull(calibrator.getEstimatedMzy());
6366 assertNull(calibrator.getEstimatedCovariance());
6367 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6368
6369
6370 calibrator = null;
6371 try {
6372 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
6373 new double[1], this);
6374 fail("IllegalArgumentException expected but not thrown");
6375 } catch (final IllegalArgumentException ignore) {
6376 }
6377 assertNull(calibrator);
6378 }
6379
6380 @Test
6381 public void testConstructor59() throws WrongSizeException {
6382 final Matrix ba = generateBa();
6383 final double[] bias = ba.getBuffer();
6384 final double biasX = ba.getElementAtIndex(0);
6385 final double biasY = ba.getElementAtIndex(1);
6386 final double biasZ = ba.getElementAtIndex(2);
6387
6388 KnownPositionAccelerometerCalibrator calibrator =
6389 new KnownPositionAccelerometerCalibrator(true, bias);
6390
6391
6392 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6393 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6394 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6395 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6396 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6397 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6398 final Acceleration bx2 = new Acceleration(0.0,
6399 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6400 calibrator.getInitialBiasXAsAcceleration(bx2);
6401 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6402 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6403 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6404 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6405 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6406 final Acceleration by2 = new Acceleration(0.0,
6407 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6408 calibrator.getInitialBiasYAsAcceleration(by2);
6409 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6410 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6411 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6412 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6413 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6414 final Acceleration bz2 = new Acceleration(0.0,
6415 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6416 calibrator.getInitialBiasZAsAcceleration(bz2);
6417 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6418 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6419 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6420 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6421 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6422 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6423 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6424 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6425 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6426 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6427 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6428 final double[] bias1 = calibrator.getInitialBias();
6429 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
6430 final double[] bias2 = new double[3];
6431 calibrator.getInitialBias(bias2);
6432 assertArrayEquals(bias1, bias2, 0.0);
6433 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6434 assertEquals(b1, ba);
6435 final Matrix b2 = new Matrix(3, 1);
6436 calibrator.getInitialBiasAsMatrix(b2);
6437 assertEquals(b1, b2);
6438 final Matrix ma1 = calibrator.getInitialMa();
6439 assertEquals(ma1, new Matrix(3, 3));
6440 final Matrix ma2 = new Matrix(3, 3);
6441 calibrator.getInitialMa(ma2);
6442 assertEquals(ma1, ma2);
6443 assertNull(calibrator.getEcefPosition());
6444 assertNull(calibrator.getNedPosition());
6445 assertFalse(calibrator.getNedPosition(null));
6446 assertNull(calibrator.getMeasurements());
6447 assertTrue(calibrator.isCommonAxisUsed());
6448 assertNull(calibrator.getListener());
6449 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6450 assertFalse(calibrator.isReady());
6451 assertFalse(calibrator.isRunning());
6452 assertNull(calibrator.getEstimatedBiases());
6453 assertFalse(calibrator.getEstimatedBiases(null));
6454 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6455 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6456 assertNull(calibrator.getEstimatedBiasFx());
6457 assertNull(calibrator.getEstimatedBiasFy());
6458 assertNull(calibrator.getEstimatedBiasFz());
6459 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6460 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6461 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6462 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6463 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6464 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6465 assertNull(calibrator.getEstimatedMa());
6466 assertNull(calibrator.getEstimatedSx());
6467 assertNull(calibrator.getEstimatedSy());
6468 assertNull(calibrator.getEstimatedSz());
6469 assertNull(calibrator.getEstimatedMxy());
6470 assertNull(calibrator.getEstimatedMxz());
6471 assertNull(calibrator.getEstimatedMyx());
6472 assertNull(calibrator.getEstimatedMyz());
6473 assertNull(calibrator.getEstimatedMzx());
6474 assertNull(calibrator.getEstimatedMzy());
6475 assertNull(calibrator.getEstimatedCovariance());
6476 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6477
6478
6479 calibrator = null;
6480 try {
6481 calibrator = new KnownPositionAccelerometerCalibrator(true,
6482 new double[1]);
6483 fail("IllegalArgumentException expected but not thrown");
6484 } catch (final IllegalArgumentException ignore) {
6485 }
6486 assertNull(calibrator);
6487 }
6488
6489 @Test
6490 public void testConstructor60() throws WrongSizeException {
6491 final Matrix ba = generateBa();
6492 final double[] bias = ba.getBuffer();
6493 final double biasX = ba.getElementAtIndex(0);
6494 final double biasY = ba.getElementAtIndex(1);
6495 final double biasZ = ba.getElementAtIndex(2);
6496
6497 KnownPositionAccelerometerCalibrator calibrator =
6498 new KnownPositionAccelerometerCalibrator(true, bias, this);
6499
6500
6501 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6502 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6503 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6504 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6505 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6506 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6507 final Acceleration bx2 = new Acceleration(0.0,
6508 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6509 calibrator.getInitialBiasXAsAcceleration(bx2);
6510 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6511 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6512 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6513 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6514 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6515 final Acceleration by2 = new Acceleration(0.0,
6516 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6517 calibrator.getInitialBiasYAsAcceleration(by2);
6518 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6519 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6520 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6521 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6522 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6523 final Acceleration bz2 = new Acceleration(0.0,
6524 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6525 calibrator.getInitialBiasZAsAcceleration(bz2);
6526 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6527 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6528 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6529 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6530 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6531 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6532 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6533 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6534 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6535 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6536 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6537 final double[] bias1 = calibrator.getInitialBias();
6538 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
6539 final double[] bias2 = new double[3];
6540 calibrator.getInitialBias(bias2);
6541 assertArrayEquals(bias1, bias2, 0.0);
6542 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6543 assertEquals(b1, ba);
6544 final Matrix b2 = new Matrix(3, 1);
6545 calibrator.getInitialBiasAsMatrix(b2);
6546 assertEquals(b1, b2);
6547 final Matrix ma1 = calibrator.getInitialMa();
6548 assertEquals(ma1, new Matrix(3, 3));
6549 final Matrix ma2 = new Matrix(3, 3);
6550 calibrator.getInitialMa(ma2);
6551 assertEquals(ma1, ma2);
6552 assertNull(calibrator.getEcefPosition());
6553 assertNull(calibrator.getNedPosition());
6554 assertFalse(calibrator.getNedPosition(null));
6555 assertNull(calibrator.getMeasurements());
6556 assertTrue(calibrator.isCommonAxisUsed());
6557 assertSame(calibrator.getListener(), this);
6558 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6559 assertFalse(calibrator.isReady());
6560 assertFalse(calibrator.isRunning());
6561 assertNull(calibrator.getEstimatedBiases());
6562 assertFalse(calibrator.getEstimatedBiases(null));
6563 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6564 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6565 assertNull(calibrator.getEstimatedBiasFx());
6566 assertNull(calibrator.getEstimatedBiasFy());
6567 assertNull(calibrator.getEstimatedBiasFz());
6568 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6569 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6570 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6571 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6572 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6573 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6574 assertNull(calibrator.getEstimatedMa());
6575 assertNull(calibrator.getEstimatedSx());
6576 assertNull(calibrator.getEstimatedSy());
6577 assertNull(calibrator.getEstimatedSz());
6578 assertNull(calibrator.getEstimatedMxy());
6579 assertNull(calibrator.getEstimatedMxz());
6580 assertNull(calibrator.getEstimatedMyx());
6581 assertNull(calibrator.getEstimatedMyz());
6582 assertNull(calibrator.getEstimatedMzx());
6583 assertNull(calibrator.getEstimatedMzy());
6584 assertNull(calibrator.getEstimatedCovariance());
6585 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6586
6587
6588 calibrator = null;
6589 try {
6590 calibrator = new KnownPositionAccelerometerCalibrator(true,
6591 new double[1], this);
6592 fail("IllegalArgumentException expected but not thrown");
6593 } catch (final IllegalArgumentException ignore) {
6594 }
6595 assertNull(calibrator);
6596 }
6597
6598 @Test
6599 public void testConstructor61() throws WrongSizeException {
6600 final Collection<StandardDeviationBodyKinematics> measurements =
6601 Collections.emptyList();
6602
6603 final Matrix ba = generateBa();
6604 final double[] bias = ba.getBuffer();
6605 final double biasX = ba.getElementAtIndex(0);
6606 final double biasY = ba.getElementAtIndex(1);
6607 final double biasZ = ba.getElementAtIndex(2);
6608
6609 KnownPositionAccelerometerCalibrator calibrator =
6610 new KnownPositionAccelerometerCalibrator(measurements,
6611 true, bias);
6612
6613
6614 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6615 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6616 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6617 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6618 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6619 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6620 final Acceleration bx2 = new Acceleration(0.0,
6621 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6622 calibrator.getInitialBiasXAsAcceleration(bx2);
6623 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6624 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6625 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6626 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6627 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6628 final Acceleration by2 = new Acceleration(0.0,
6629 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6630 calibrator.getInitialBiasYAsAcceleration(by2);
6631 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6632 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6633 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6634 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6635 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6636 final Acceleration bz2 = new Acceleration(0.0,
6637 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6638 calibrator.getInitialBiasZAsAcceleration(bz2);
6639 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6640 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6641 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6642 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6643 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6644 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6645 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6646 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6647 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6648 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6649 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6650 final double[] bias1 = calibrator.getInitialBias();
6651 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
6652 final double[] bias2 = new double[3];
6653 calibrator.getInitialBias(bias2);
6654 assertArrayEquals(bias1, bias2, 0.0);
6655 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6656 assertEquals(b1, ba);
6657 final Matrix b2 = new Matrix(3, 1);
6658 calibrator.getInitialBiasAsMatrix(b2);
6659 assertEquals(b1, b2);
6660 final Matrix ma1 = calibrator.getInitialMa();
6661 assertEquals(ma1, new Matrix(3, 3));
6662 final Matrix ma2 = new Matrix(3, 3);
6663 calibrator.getInitialMa(ma2);
6664 assertEquals(ma1, ma2);
6665 assertNull(calibrator.getEcefPosition());
6666 assertNull(calibrator.getNedPosition());
6667 assertFalse(calibrator.getNedPosition(null));
6668 assertSame(calibrator.getMeasurements(), measurements);
6669 assertTrue(calibrator.isCommonAxisUsed());
6670 assertNull(calibrator.getListener());
6671 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6672 assertFalse(calibrator.isReady());
6673 assertFalse(calibrator.isRunning());
6674 assertNull(calibrator.getEstimatedBiases());
6675 assertFalse(calibrator.getEstimatedBiases(null));
6676 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6677 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6678 assertNull(calibrator.getEstimatedBiasFx());
6679 assertNull(calibrator.getEstimatedBiasFy());
6680 assertNull(calibrator.getEstimatedBiasFz());
6681 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6682 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6683 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6684 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6685 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6686 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6687 assertNull(calibrator.getEstimatedMa());
6688 assertNull(calibrator.getEstimatedSx());
6689 assertNull(calibrator.getEstimatedSy());
6690 assertNull(calibrator.getEstimatedSz());
6691 assertNull(calibrator.getEstimatedMxy());
6692 assertNull(calibrator.getEstimatedMxz());
6693 assertNull(calibrator.getEstimatedMyx());
6694 assertNull(calibrator.getEstimatedMyz());
6695 assertNull(calibrator.getEstimatedMzx());
6696 assertNull(calibrator.getEstimatedMzy());
6697 assertNull(calibrator.getEstimatedCovariance());
6698 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6699
6700
6701 calibrator = null;
6702 try {
6703 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
6704 true, new double[1]);
6705 fail("IllegalArgumentException expected but not thrown");
6706 } catch (final IllegalArgumentException ignore) {
6707 }
6708 assertNull(calibrator);
6709 }
6710
6711 @Test
6712 public void testConstructor62() throws WrongSizeException {
6713 final Collection<StandardDeviationBodyKinematics> measurements =
6714 Collections.emptyList();
6715
6716 final Matrix ba = generateBa();
6717 final double[] bias = ba.getBuffer();
6718 final double biasX = ba.getElementAtIndex(0);
6719 final double biasY = ba.getElementAtIndex(1);
6720 final double biasZ = ba.getElementAtIndex(2);
6721
6722 KnownPositionAccelerometerCalibrator calibrator =
6723 new KnownPositionAccelerometerCalibrator(measurements,
6724 true, bias, this);
6725
6726
6727 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6728 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6729 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6730 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6731 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6732 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6733 final Acceleration bx2 = new Acceleration(0.0,
6734 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6735 calibrator.getInitialBiasXAsAcceleration(bx2);
6736 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6737 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6738 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6739 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6740 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6741 final Acceleration by2 = new Acceleration(0.0,
6742 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6743 calibrator.getInitialBiasYAsAcceleration(by2);
6744 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6745 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6746 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6747 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6748 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6749 final Acceleration bz2 = new Acceleration(0.0,
6750 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6751 calibrator.getInitialBiasZAsAcceleration(bz2);
6752 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6753 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6754 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6755 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6756 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6757 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6758 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6759 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6760 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6761 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6762 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6763 final double[] bias1 = calibrator.getInitialBias();
6764 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
6765 final double[] bias2 = new double[3];
6766 calibrator.getInitialBias(bias2);
6767 assertArrayEquals(bias1, bias2, 0.0);
6768 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6769 assertEquals(b1, ba);
6770 final Matrix b2 = new Matrix(3, 1);
6771 calibrator.getInitialBiasAsMatrix(b2);
6772 assertEquals(b1, b2);
6773 final Matrix ma1 = calibrator.getInitialMa();
6774 assertEquals(ma1, new Matrix(3, 3));
6775 final Matrix ma2 = new Matrix(3, 3);
6776 calibrator.getInitialMa(ma2);
6777 assertEquals(ma1, ma2);
6778 assertNull(calibrator.getEcefPosition());
6779 assertNull(calibrator.getNedPosition());
6780 assertFalse(calibrator.getNedPosition(null));
6781 assertSame(calibrator.getMeasurements(), measurements);
6782 assertTrue(calibrator.isCommonAxisUsed());
6783 assertSame(calibrator.getListener(), this);
6784 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6785 assertFalse(calibrator.isReady());
6786 assertFalse(calibrator.isRunning());
6787 assertNull(calibrator.getEstimatedBiases());
6788 assertFalse(calibrator.getEstimatedBiases(null));
6789 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6790 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6791 assertNull(calibrator.getEstimatedBiasFx());
6792 assertNull(calibrator.getEstimatedBiasFy());
6793 assertNull(calibrator.getEstimatedBiasFz());
6794 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6795 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6796 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6797 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6798 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6799 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6800 assertNull(calibrator.getEstimatedMa());
6801 assertNull(calibrator.getEstimatedSx());
6802 assertNull(calibrator.getEstimatedSy());
6803 assertNull(calibrator.getEstimatedSz());
6804 assertNull(calibrator.getEstimatedMxy());
6805 assertNull(calibrator.getEstimatedMxz());
6806 assertNull(calibrator.getEstimatedMyx());
6807 assertNull(calibrator.getEstimatedMyz());
6808 assertNull(calibrator.getEstimatedMzx());
6809 assertNull(calibrator.getEstimatedMzy());
6810 assertNull(calibrator.getEstimatedCovariance());
6811 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6812
6813
6814 calibrator = null;
6815 try {
6816 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
6817 true, new double[1], this);
6818 fail("IllegalArgumentException expected but not thrown");
6819 } catch (final IllegalArgumentException ignore) {
6820 }
6821 assertNull(calibrator);
6822 }
6823
6824 @Test
6825 public void testConstructor63() throws WrongSizeException {
6826 final Matrix ba = generateBa();
6827 final double biasX = ba.getElementAtIndex(0);
6828 final double biasY = ba.getElementAtIndex(1);
6829 final double biasZ = ba.getElementAtIndex(2);
6830
6831 KnownPositionAccelerometerCalibrator calibrator =
6832 new KnownPositionAccelerometerCalibrator(ba);
6833
6834
6835 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6836 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6837 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6838 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6839 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6840 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6841 final Acceleration bx2 = new Acceleration(0.0,
6842 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6843 calibrator.getInitialBiasXAsAcceleration(bx2);
6844 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6845 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6846 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6847 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6848 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6849 final Acceleration by2 = new Acceleration(0.0,
6850 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6851 calibrator.getInitialBiasYAsAcceleration(by2);
6852 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6853 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6854 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6855 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6856 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6857 final Acceleration bz2 = new Acceleration(0.0,
6858 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6859 calibrator.getInitialBiasZAsAcceleration(bz2);
6860 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6861 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6862 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6863 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6864 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6865 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6866 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6867 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6868 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6869 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6870 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6871 final double[] bias1 = calibrator.getInitialBias();
6872 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
6873 final double[] bias2 = new double[3];
6874 calibrator.getInitialBias(bias2);
6875 assertArrayEquals(bias1, bias2, 0.0);
6876 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6877 assertEquals(b1, ba);
6878 final Matrix b2 = new Matrix(3, 1);
6879 calibrator.getInitialBiasAsMatrix(b2);
6880 assertEquals(b1, b2);
6881 final Matrix ma1 = calibrator.getInitialMa();
6882 assertEquals(ma1, new Matrix(3, 3));
6883 final Matrix ma2 = new Matrix(3, 3);
6884 calibrator.getInitialMa(ma2);
6885 assertEquals(ma1, ma2);
6886 assertNull(calibrator.getEcefPosition());
6887 assertNull(calibrator.getNedPosition());
6888 assertFalse(calibrator.getNedPosition(null));
6889 assertNull(calibrator.getMeasurements());
6890 assertFalse(calibrator.isCommonAxisUsed());
6891 assertNull(calibrator.getListener());
6892 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6893 assertFalse(calibrator.isReady());
6894 assertFalse(calibrator.isRunning());
6895 assertNull(calibrator.getEstimatedBiases());
6896 assertFalse(calibrator.getEstimatedBiases(null));
6897 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6898 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6899 assertNull(calibrator.getEstimatedBiasFx());
6900 assertNull(calibrator.getEstimatedBiasFy());
6901 assertNull(calibrator.getEstimatedBiasFz());
6902 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6903 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6904 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6905 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6906 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6907 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6908 assertNull(calibrator.getEstimatedMa());
6909 assertNull(calibrator.getEstimatedSx());
6910 assertNull(calibrator.getEstimatedSy());
6911 assertNull(calibrator.getEstimatedSz());
6912 assertNull(calibrator.getEstimatedMxy());
6913 assertNull(calibrator.getEstimatedMxz());
6914 assertNull(calibrator.getEstimatedMyx());
6915 assertNull(calibrator.getEstimatedMyz());
6916 assertNull(calibrator.getEstimatedMzx());
6917 assertNull(calibrator.getEstimatedMzy());
6918 assertNull(calibrator.getEstimatedCovariance());
6919 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6920
6921
6922 calibrator = null;
6923 try {
6924 calibrator = new KnownPositionAccelerometerCalibrator(
6925 new Matrix(1, 1));
6926 fail("IllegalArgumentException expected but not thrown");
6927 } catch (final IllegalArgumentException ignore) {
6928 }
6929 try {
6930 calibrator = new KnownPositionAccelerometerCalibrator(
6931 new Matrix(1, 3));
6932 fail("IllegalArgumentException expected but not thrown");
6933 } catch (final IllegalArgumentException ignore) {
6934 }
6935 assertNull(calibrator);
6936 }
6937
6938 @Test
6939 public void testConstructor64() throws WrongSizeException {
6940 final Matrix ba = generateBa();
6941 final double biasX = ba.getElementAtIndex(0);
6942 final double biasY = ba.getElementAtIndex(1);
6943 final double biasZ = ba.getElementAtIndex(2);
6944
6945 KnownPositionAccelerometerCalibrator calibrator =
6946 new KnownPositionAccelerometerCalibrator(ba, this);
6947
6948
6949 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6950 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6951 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6952 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6953 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6954 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6955 final Acceleration bx2 = new Acceleration(0.0,
6956 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6957 calibrator.getInitialBiasXAsAcceleration(bx2);
6958 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6959 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6960 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6961 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6962 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6963 final Acceleration by2 = new Acceleration(0.0,
6964 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6965 calibrator.getInitialBiasYAsAcceleration(by2);
6966 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6967 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6968 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6969 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6970 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6971 final Acceleration bz2 = new Acceleration(0.0,
6972 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6973 calibrator.getInitialBiasZAsAcceleration(bz2);
6974 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6975 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6976 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6977 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6978 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6979 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6980 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6981 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6982 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6983 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6984 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6985 final double[] bias1 = calibrator.getInitialBias();
6986 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
6987 final double[] bias2 = new double[3];
6988 calibrator.getInitialBias(bias2);
6989 assertArrayEquals(bias1, bias2, 0.0);
6990 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6991 assertEquals(b1, ba);
6992 final Matrix b2 = new Matrix(3, 1);
6993 calibrator.getInitialBiasAsMatrix(b2);
6994 assertEquals(b1, b2);
6995 final Matrix ma1 = calibrator.getInitialMa();
6996 assertEquals(ma1, new Matrix(3, 3));
6997 final Matrix ma2 = new Matrix(3, 3);
6998 calibrator.getInitialMa(ma2);
6999 assertEquals(ma1, ma2);
7000 assertNull(calibrator.getEcefPosition());
7001 assertNull(calibrator.getNedPosition());
7002 assertFalse(calibrator.getNedPosition(null));
7003 assertNull(calibrator.getMeasurements());
7004 assertFalse(calibrator.isCommonAxisUsed());
7005 assertSame(calibrator.getListener(), this);
7006 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7007 assertFalse(calibrator.isReady());
7008 assertFalse(calibrator.isRunning());
7009 assertNull(calibrator.getEstimatedBiases());
7010 assertFalse(calibrator.getEstimatedBiases(null));
7011 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7012 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7013 assertNull(calibrator.getEstimatedBiasFx());
7014 assertNull(calibrator.getEstimatedBiasFy());
7015 assertNull(calibrator.getEstimatedBiasFz());
7016 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7017 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7018 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7019 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7020 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7021 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7022 assertNull(calibrator.getEstimatedMa());
7023 assertNull(calibrator.getEstimatedSx());
7024 assertNull(calibrator.getEstimatedSy());
7025 assertNull(calibrator.getEstimatedSz());
7026 assertNull(calibrator.getEstimatedMxy());
7027 assertNull(calibrator.getEstimatedMxz());
7028 assertNull(calibrator.getEstimatedMyx());
7029 assertNull(calibrator.getEstimatedMyz());
7030 assertNull(calibrator.getEstimatedMzx());
7031 assertNull(calibrator.getEstimatedMzy());
7032 assertNull(calibrator.getEstimatedCovariance());
7033 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7034
7035
7036 calibrator = null;
7037 try {
7038 calibrator = new KnownPositionAccelerometerCalibrator(
7039 new Matrix(1, 1));
7040 fail("IllegalArgumentException expected but not thrown");
7041 } catch (final IllegalArgumentException ignore) {
7042 }
7043 try {
7044 calibrator = new KnownPositionAccelerometerCalibrator(
7045 new Matrix(1, 3));
7046 fail("IllegalArgumentException expected but not thrown");
7047 } catch (final IllegalArgumentException ignore) {
7048 }
7049 assertNull(calibrator);
7050 }
7051
7052 @Test
7053 public void testConstructor65() throws WrongSizeException {
7054 final Collection<StandardDeviationBodyKinematics> measurements =
7055 Collections.emptyList();
7056
7057 final Matrix ba = generateBa();
7058 final double biasX = ba.getElementAtIndex(0);
7059 final double biasY = ba.getElementAtIndex(1);
7060 final double biasZ = ba.getElementAtIndex(2);
7061
7062 KnownPositionAccelerometerCalibrator calibrator =
7063 new KnownPositionAccelerometerCalibrator(measurements, ba);
7064
7065
7066 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7067 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7068 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7069 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7070 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7071 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7072 final Acceleration bx2 = new Acceleration(0.0,
7073 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7074 calibrator.getInitialBiasXAsAcceleration(bx2);
7075 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7076 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7077 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7078 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7079 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7080 final Acceleration by2 = new Acceleration(0.0,
7081 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7082 calibrator.getInitialBiasYAsAcceleration(by2);
7083 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7084 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7085 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7086 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7087 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7088 final Acceleration bz2 = new Acceleration(0.0,
7089 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7090 calibrator.getInitialBiasZAsAcceleration(bz2);
7091 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7092 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7093 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7094 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7095 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7096 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7097 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7098 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7099 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7100 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7101 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7102 final double[] bias1 = calibrator.getInitialBias();
7103 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
7104 final double[] bias2 = new double[3];
7105 calibrator.getInitialBias(bias2);
7106 assertArrayEquals(bias1, bias2, 0.0);
7107 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7108 assertEquals(b1, ba);
7109 final Matrix b2 = new Matrix(3, 1);
7110 calibrator.getInitialBiasAsMatrix(b2);
7111 assertEquals(b1, b2);
7112 final Matrix ma1 = calibrator.getInitialMa();
7113 assertEquals(ma1, new Matrix(3, 3));
7114 final Matrix ma2 = new Matrix(3, 3);
7115 calibrator.getInitialMa(ma2);
7116 assertEquals(ma1, ma2);
7117 assertNull(calibrator.getEcefPosition());
7118 assertNull(calibrator.getNedPosition());
7119 assertFalse(calibrator.getNedPosition(null));
7120 assertSame(calibrator.getMeasurements(), measurements);
7121 assertFalse(calibrator.isCommonAxisUsed());
7122 assertNull(calibrator.getListener());
7123 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7124 assertFalse(calibrator.isReady());
7125 assertFalse(calibrator.isRunning());
7126 assertNull(calibrator.getEstimatedBiases());
7127 assertFalse(calibrator.getEstimatedBiases(null));
7128 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7129 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7130 assertNull(calibrator.getEstimatedBiasFx());
7131 assertNull(calibrator.getEstimatedBiasFy());
7132 assertNull(calibrator.getEstimatedBiasFz());
7133 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7134 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7135 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7136 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7137 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7138 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7139 assertNull(calibrator.getEstimatedMa());
7140 assertNull(calibrator.getEstimatedSx());
7141 assertNull(calibrator.getEstimatedSy());
7142 assertNull(calibrator.getEstimatedSz());
7143 assertNull(calibrator.getEstimatedMxy());
7144 assertNull(calibrator.getEstimatedMxz());
7145 assertNull(calibrator.getEstimatedMyx());
7146 assertNull(calibrator.getEstimatedMyz());
7147 assertNull(calibrator.getEstimatedMzx());
7148 assertNull(calibrator.getEstimatedMzy());
7149 assertNull(calibrator.getEstimatedCovariance());
7150 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7151
7152
7153 calibrator = null;
7154 try {
7155 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7156 new Matrix(1, 1));
7157 fail("IllegalArgumentException expected but not thrown");
7158 } catch (final IllegalArgumentException ignore) {
7159 }
7160 try {
7161 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7162 new Matrix(1, 3));
7163 fail("IllegalArgumentException expected but not thrown");
7164 } catch (final IllegalArgumentException ignore) {
7165 }
7166 assertNull(calibrator);
7167 }
7168
7169 @Test
7170 public void testConstructor66() throws WrongSizeException {
7171 final Collection<StandardDeviationBodyKinematics> measurements =
7172 Collections.emptyList();
7173
7174 final Matrix ba = generateBa();
7175 final double biasX = ba.getElementAtIndex(0);
7176 final double biasY = ba.getElementAtIndex(1);
7177 final double biasZ = ba.getElementAtIndex(2);
7178
7179 KnownPositionAccelerometerCalibrator calibrator =
7180 new KnownPositionAccelerometerCalibrator(measurements, ba,
7181 this);
7182
7183
7184 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7185 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7186 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7187 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7188 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7189 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7190 final Acceleration bx2 = new Acceleration(0.0,
7191 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7192 calibrator.getInitialBiasXAsAcceleration(bx2);
7193 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7194 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7195 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7196 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7197 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7198 final Acceleration by2 = new Acceleration(0.0,
7199 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7200 calibrator.getInitialBiasYAsAcceleration(by2);
7201 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7202 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7203 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7204 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7205 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7206 final Acceleration bz2 = new Acceleration(0.0,
7207 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7208 calibrator.getInitialBiasZAsAcceleration(bz2);
7209 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7210 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7211 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7212 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7213 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7214 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7215 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7216 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7217 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7218 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7219 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7220 final double[] bias1 = calibrator.getInitialBias();
7221 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
7222 final double[] bias2 = new double[3];
7223 calibrator.getInitialBias(bias2);
7224 assertArrayEquals(bias1, bias2, 0.0);
7225 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7226 assertEquals(b1, ba);
7227 final Matrix b2 = new Matrix(3, 1);
7228 calibrator.getInitialBiasAsMatrix(b2);
7229 assertEquals(b1, b2);
7230 final Matrix ma1 = calibrator.getInitialMa();
7231 assertEquals(ma1, new Matrix(3, 3));
7232 final Matrix ma2 = new Matrix(3, 3);
7233 calibrator.getInitialMa(ma2);
7234 assertEquals(ma1, ma2);
7235 assertNull(calibrator.getEcefPosition());
7236 assertNull(calibrator.getNedPosition());
7237 assertFalse(calibrator.getNedPosition(null));
7238 assertSame(calibrator.getMeasurements(), measurements);
7239 assertFalse(calibrator.isCommonAxisUsed());
7240 assertSame(calibrator.getListener(), this);
7241 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7242 assertFalse(calibrator.isReady());
7243 assertFalse(calibrator.isRunning());
7244 assertNull(calibrator.getEstimatedBiases());
7245 assertFalse(calibrator.getEstimatedBiases(null));
7246 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7247 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7248 assertNull(calibrator.getEstimatedBiasFx());
7249 assertNull(calibrator.getEstimatedBiasFy());
7250 assertNull(calibrator.getEstimatedBiasFz());
7251 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7252 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7253 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7254 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7255 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7256 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7257 assertNull(calibrator.getEstimatedMa());
7258 assertNull(calibrator.getEstimatedSx());
7259 assertNull(calibrator.getEstimatedSy());
7260 assertNull(calibrator.getEstimatedSz());
7261 assertNull(calibrator.getEstimatedMxy());
7262 assertNull(calibrator.getEstimatedMxz());
7263 assertNull(calibrator.getEstimatedMyx());
7264 assertNull(calibrator.getEstimatedMyz());
7265 assertNull(calibrator.getEstimatedMzx());
7266 assertNull(calibrator.getEstimatedMzy());
7267 assertNull(calibrator.getEstimatedCovariance());
7268 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7269
7270
7271 calibrator = null;
7272 try {
7273 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7274 new Matrix(1, 1), this);
7275 fail("IllegalArgumentException expected but not thrown");
7276 } catch (final IllegalArgumentException ignore) {
7277 }
7278 try {
7279 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7280 new Matrix(1, 3), this);
7281 fail("IllegalArgumentException expected but not thrown");
7282 } catch (final IllegalArgumentException ignore) {
7283 }
7284 assertNull(calibrator);
7285 }
7286
7287 @Test
7288 public void testConstructor67() throws WrongSizeException {
7289 final Matrix ba = generateBa();
7290 final double biasX = ba.getElementAtIndex(0);
7291 final double biasY = ba.getElementAtIndex(1);
7292 final double biasZ = ba.getElementAtIndex(2);
7293
7294 KnownPositionAccelerometerCalibrator calibrator =
7295 new KnownPositionAccelerometerCalibrator(true, ba);
7296
7297
7298 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7299 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7300 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7301 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7302 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7303 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7304 final Acceleration bx2 = new Acceleration(0.0,
7305 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7306 calibrator.getInitialBiasXAsAcceleration(bx2);
7307 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7308 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7309 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7310 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7311 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7312 final Acceleration by2 = new Acceleration(0.0,
7313 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7314 calibrator.getInitialBiasYAsAcceleration(by2);
7315 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7316 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7317 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7318 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7319 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7320 final Acceleration bz2 = new Acceleration(0.0,
7321 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7322 calibrator.getInitialBiasZAsAcceleration(bz2);
7323 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7324 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7325 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7326 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7327 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7328 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7329 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7330 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7331 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7332 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7333 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7334 final double[] bias1 = calibrator.getInitialBias();
7335 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
7336 final double[] bias2 = new double[3];
7337 calibrator.getInitialBias(bias2);
7338 assertArrayEquals(bias1, bias2, 0.0);
7339 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7340 assertEquals(b1, ba);
7341 final Matrix b2 = new Matrix(3, 1);
7342 calibrator.getInitialBiasAsMatrix(b2);
7343 assertEquals(b1, b2);
7344 final Matrix ma1 = calibrator.getInitialMa();
7345 assertEquals(ma1, new Matrix(3, 3));
7346 final Matrix ma2 = new Matrix(3, 3);
7347 calibrator.getInitialMa(ma2);
7348 assertEquals(ma1, ma2);
7349 assertNull(calibrator.getEcefPosition());
7350 assertNull(calibrator.getNedPosition());
7351 assertFalse(calibrator.getNedPosition(null));
7352 assertNull(calibrator.getMeasurements());
7353 assertTrue(calibrator.isCommonAxisUsed());
7354 assertNull(calibrator.getListener());
7355 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7356 assertFalse(calibrator.isReady());
7357 assertFalse(calibrator.isRunning());
7358 assertNull(calibrator.getEstimatedBiases());
7359 assertFalse(calibrator.getEstimatedBiases(null));
7360 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7361 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7362 assertNull(calibrator.getEstimatedBiasFx());
7363 assertNull(calibrator.getEstimatedBiasFy());
7364 assertNull(calibrator.getEstimatedBiasFz());
7365 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7366 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7367 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7368 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7369 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7370 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7371 assertNull(calibrator.getEstimatedMa());
7372 assertNull(calibrator.getEstimatedSx());
7373 assertNull(calibrator.getEstimatedSy());
7374 assertNull(calibrator.getEstimatedSz());
7375 assertNull(calibrator.getEstimatedMxy());
7376 assertNull(calibrator.getEstimatedMxz());
7377 assertNull(calibrator.getEstimatedMyx());
7378 assertNull(calibrator.getEstimatedMyz());
7379 assertNull(calibrator.getEstimatedMzx());
7380 assertNull(calibrator.getEstimatedMzy());
7381 assertNull(calibrator.getEstimatedCovariance());
7382 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7383
7384
7385 calibrator = null;
7386 try {
7387 calibrator = new KnownPositionAccelerometerCalibrator(
7388 true, new Matrix(1, 1));
7389 fail("IllegalArgumentException expected but not thrown");
7390 } catch (final IllegalArgumentException ignore) {
7391 }
7392 try {
7393 calibrator = new KnownPositionAccelerometerCalibrator(
7394 true, new Matrix(1, 3));
7395 fail("IllegalArgumentException expected but not thrown");
7396 } catch (final IllegalArgumentException ignore) {
7397 }
7398 assertNull(calibrator);
7399 }
7400
7401 @Test
7402 public void testConstructor68() throws WrongSizeException {
7403 final Matrix ba = generateBa();
7404 final double biasX = ba.getElementAtIndex(0);
7405 final double biasY = ba.getElementAtIndex(1);
7406 final double biasZ = ba.getElementAtIndex(2);
7407
7408 KnownPositionAccelerometerCalibrator calibrator =
7409 new KnownPositionAccelerometerCalibrator(true, ba,
7410 this);
7411
7412
7413 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7414 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7415 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7416 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7417 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7418 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7419 final Acceleration bx2 = new Acceleration(0.0,
7420 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7421 calibrator.getInitialBiasXAsAcceleration(bx2);
7422 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7423 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7424 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7425 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7426 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7427 final Acceleration by2 = new Acceleration(0.0,
7428 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7429 calibrator.getInitialBiasYAsAcceleration(by2);
7430 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7431 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7432 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7433 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7434 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7435 final Acceleration bz2 = new Acceleration(0.0,
7436 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7437 calibrator.getInitialBiasZAsAcceleration(bz2);
7438 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7439 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7440 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7441 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7442 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7443 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7444 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7445 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7446 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7447 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7448 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7449 final double[] bias1 = calibrator.getInitialBias();
7450 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
7451 final double[] bias2 = new double[3];
7452 calibrator.getInitialBias(bias2);
7453 assertArrayEquals(bias1, bias2, 0.0);
7454 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7455 assertEquals(b1, ba);
7456 final Matrix b2 = new Matrix(3, 1);
7457 calibrator.getInitialBiasAsMatrix(b2);
7458 assertEquals(b1, b2);
7459 final Matrix ma1 = calibrator.getInitialMa();
7460 assertEquals(ma1, new Matrix(3, 3));
7461 final Matrix ma2 = new Matrix(3, 3);
7462 calibrator.getInitialMa(ma2);
7463 assertEquals(ma1, ma2);
7464 assertNull(calibrator.getEcefPosition());
7465 assertNull(calibrator.getNedPosition());
7466 assertFalse(calibrator.getNedPosition(null));
7467 assertNull(calibrator.getMeasurements());
7468 assertTrue(calibrator.isCommonAxisUsed());
7469 assertSame(calibrator.getListener(), this);
7470 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7471 assertFalse(calibrator.isReady());
7472 assertFalse(calibrator.isRunning());
7473 assertNull(calibrator.getEstimatedBiases());
7474 assertFalse(calibrator.getEstimatedBiases(null));
7475 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7476 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7477 assertNull(calibrator.getEstimatedBiasFx());
7478 assertNull(calibrator.getEstimatedBiasFy());
7479 assertNull(calibrator.getEstimatedBiasFz());
7480 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7481 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7482 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7483 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7484 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7485 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7486 assertNull(calibrator.getEstimatedMa());
7487 assertNull(calibrator.getEstimatedSx());
7488 assertNull(calibrator.getEstimatedSy());
7489 assertNull(calibrator.getEstimatedSz());
7490 assertNull(calibrator.getEstimatedMxy());
7491 assertNull(calibrator.getEstimatedMxz());
7492 assertNull(calibrator.getEstimatedMyx());
7493 assertNull(calibrator.getEstimatedMyz());
7494 assertNull(calibrator.getEstimatedMzx());
7495 assertNull(calibrator.getEstimatedMzy());
7496 assertNull(calibrator.getEstimatedCovariance());
7497 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7498
7499
7500 calibrator = null;
7501 try {
7502 calibrator = new KnownPositionAccelerometerCalibrator(
7503 true, new Matrix(1, 1),
7504 this);
7505 fail("IllegalArgumentException expected but not thrown");
7506 } catch (final IllegalArgumentException ignore) {
7507 }
7508 try {
7509 calibrator = new KnownPositionAccelerometerCalibrator(
7510 true, new Matrix(1, 3),
7511 this);
7512 fail("IllegalArgumentException expected but not thrown");
7513 } catch (final IllegalArgumentException ignore) {
7514 }
7515 assertNull(calibrator);
7516 }
7517
7518 @Test
7519 public void testConstructor69() throws WrongSizeException {
7520 final Collection<StandardDeviationBodyKinematics> measurements =
7521 Collections.emptyList();
7522
7523 final Matrix ba = generateBa();
7524 final double biasX = ba.getElementAtIndex(0);
7525 final double biasY = ba.getElementAtIndex(1);
7526 final double biasZ = ba.getElementAtIndex(2);
7527
7528 KnownPositionAccelerometerCalibrator calibrator =
7529 new KnownPositionAccelerometerCalibrator(measurements,
7530 true, ba);
7531
7532
7533 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7534 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7535 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7536 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7537 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7538 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7539 final Acceleration bx2 = new Acceleration(0.0,
7540 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7541 calibrator.getInitialBiasXAsAcceleration(bx2);
7542 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7543 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7544 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7545 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7546 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7547 final Acceleration by2 = new Acceleration(0.0,
7548 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7549 calibrator.getInitialBiasYAsAcceleration(by2);
7550 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7551 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7552 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7553 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7554 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7555 final Acceleration bz2 = new Acceleration(0.0,
7556 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7557 calibrator.getInitialBiasZAsAcceleration(bz2);
7558 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7559 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7560 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7561 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7562 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7563 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7564 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7565 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7566 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7567 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7568 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7569 final double[] bias1 = calibrator.getInitialBias();
7570 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
7571 final double[] bias2 = new double[3];
7572 calibrator.getInitialBias(bias2);
7573 assertArrayEquals(bias1, bias2, 0.0);
7574 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7575 assertEquals(b1, ba);
7576 final Matrix b2 = new Matrix(3, 1);
7577 calibrator.getInitialBiasAsMatrix(b2);
7578 assertEquals(b1, b2);
7579 final Matrix ma1 = calibrator.getInitialMa();
7580 assertEquals(ma1, new Matrix(3, 3));
7581 final Matrix ma2 = new Matrix(3, 3);
7582 calibrator.getInitialMa(ma2);
7583 assertEquals(ma1, ma2);
7584 assertNull(calibrator.getEcefPosition());
7585 assertNull(calibrator.getNedPosition());
7586 assertFalse(calibrator.getNedPosition(null));
7587 assertSame(calibrator.getMeasurements(), measurements);
7588 assertTrue(calibrator.isCommonAxisUsed());
7589 assertNull(calibrator.getListener());
7590 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7591 assertFalse(calibrator.isReady());
7592 assertFalse(calibrator.isRunning());
7593 assertNull(calibrator.getEstimatedBiases());
7594 assertFalse(calibrator.getEstimatedBiases(null));
7595 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7596 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7597 assertNull(calibrator.getEstimatedBiasFx());
7598 assertNull(calibrator.getEstimatedBiasFy());
7599 assertNull(calibrator.getEstimatedBiasFz());
7600 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7601 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7602 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7603 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7604 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7605 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7606 assertNull(calibrator.getEstimatedMa());
7607 assertNull(calibrator.getEstimatedSx());
7608 assertNull(calibrator.getEstimatedSy());
7609 assertNull(calibrator.getEstimatedSz());
7610 assertNull(calibrator.getEstimatedMxy());
7611 assertNull(calibrator.getEstimatedMxz());
7612 assertNull(calibrator.getEstimatedMyx());
7613 assertNull(calibrator.getEstimatedMyz());
7614 assertNull(calibrator.getEstimatedMzx());
7615 assertNull(calibrator.getEstimatedMzy());
7616 assertNull(calibrator.getEstimatedCovariance());
7617 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7618
7619
7620 calibrator = null;
7621 try {
7622 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7623 true, new Matrix(1, 1));
7624 fail("IllegalArgumentException expected but not thrown");
7625 } catch (final IllegalArgumentException ignore) {
7626 }
7627 try {
7628 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7629 true, new Matrix(1, 3));
7630 fail("IllegalArgumentException expected but not thrown");
7631 } catch (final IllegalArgumentException ignore) {
7632 }
7633 assertNull(calibrator);
7634 }
7635
7636 @Test
7637 public void testConstructor70() throws WrongSizeException {
7638 final Collection<StandardDeviationBodyKinematics> measurements =
7639 Collections.emptyList();
7640
7641 final Matrix ba = generateBa();
7642 final double biasX = ba.getElementAtIndex(0);
7643 final double biasY = ba.getElementAtIndex(1);
7644 final double biasZ = ba.getElementAtIndex(2);
7645
7646 KnownPositionAccelerometerCalibrator calibrator =
7647 new KnownPositionAccelerometerCalibrator(measurements,
7648 true, ba, this);
7649
7650
7651 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7652 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7653 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7654 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7655 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7656 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7657 final Acceleration bx2 = new Acceleration(0.0,
7658 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7659 calibrator.getInitialBiasXAsAcceleration(bx2);
7660 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7661 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7662 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7663 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7664 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7665 final Acceleration by2 = new Acceleration(0.0,
7666 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7667 calibrator.getInitialBiasYAsAcceleration(by2);
7668 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7669 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7670 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7671 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7672 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7673 final Acceleration bz2 = new Acceleration(0.0,
7674 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7675 calibrator.getInitialBiasZAsAcceleration(bz2);
7676 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7677 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7678 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7679 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7680 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7681 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7682 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7683 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7684 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7685 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7686 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7687 final double[] bias1 = calibrator.getInitialBias();
7688 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
7689 final double[] bias2 = new double[3];
7690 calibrator.getInitialBias(bias2);
7691 assertArrayEquals(bias1, bias2, 0.0);
7692 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7693 assertEquals(b1, ba);
7694 final Matrix b2 = new Matrix(3, 1);
7695 calibrator.getInitialBiasAsMatrix(b2);
7696 assertEquals(b1, b2);
7697 final Matrix ma1 = calibrator.getInitialMa();
7698 assertEquals(ma1, new Matrix(3, 3));
7699 final Matrix ma2 = new Matrix(3, 3);
7700 calibrator.getInitialMa(ma2);
7701 assertEquals(ma1, ma2);
7702 assertNull(calibrator.getEcefPosition());
7703 assertNull(calibrator.getNedPosition());
7704 assertFalse(calibrator.getNedPosition(null));
7705 assertSame(calibrator.getMeasurements(), measurements);
7706 assertTrue(calibrator.isCommonAxisUsed());
7707 assertSame(calibrator.getListener(), this);
7708 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7709 assertFalse(calibrator.isReady());
7710 assertFalse(calibrator.isRunning());
7711 assertNull(calibrator.getEstimatedBiases());
7712 assertFalse(calibrator.getEstimatedBiases(null));
7713 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7714 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7715 assertNull(calibrator.getEstimatedBiasFx());
7716 assertNull(calibrator.getEstimatedBiasFy());
7717 assertNull(calibrator.getEstimatedBiasFz());
7718 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7719 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7720 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7721 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7722 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7723 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7724 assertNull(calibrator.getEstimatedMa());
7725 assertNull(calibrator.getEstimatedSx());
7726 assertNull(calibrator.getEstimatedSy());
7727 assertNull(calibrator.getEstimatedSz());
7728 assertNull(calibrator.getEstimatedMxy());
7729 assertNull(calibrator.getEstimatedMxz());
7730 assertNull(calibrator.getEstimatedMyx());
7731 assertNull(calibrator.getEstimatedMyz());
7732 assertNull(calibrator.getEstimatedMzx());
7733 assertNull(calibrator.getEstimatedMzy());
7734 assertNull(calibrator.getEstimatedCovariance());
7735 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7736
7737
7738 calibrator = null;
7739 try {
7740 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7741 true, new Matrix(1, 1),
7742 this);
7743 fail("IllegalArgumentException expected but not thrown");
7744 } catch (final IllegalArgumentException ignore) {
7745 }
7746 try {
7747 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7748 true, new Matrix(1, 3),
7749 this);
7750 fail("IllegalArgumentException expected but not thrown");
7751 } catch (final IllegalArgumentException ignore) {
7752 }
7753 assertNull(calibrator);
7754 }
7755
7756 @Test
7757 public void testConstructor71() throws WrongSizeException {
7758 final Matrix ba = generateBa();
7759 final double biasX = ba.getElementAtIndex(0);
7760 final double biasY = ba.getElementAtIndex(1);
7761 final double biasZ = ba.getElementAtIndex(2);
7762
7763 final Matrix ma = generateMaCommonAxis();
7764 final double sx = ma.getElementAt(0, 0);
7765 final double sy = ma.getElementAt(1, 1);
7766 final double sz = ma.getElementAt(2, 2);
7767 final double mxy = ma.getElementAt(0, 1);
7768 final double mxz = ma.getElementAt(0, 2);
7769 final double myx = ma.getElementAt(1, 0);
7770 final double myz = ma.getElementAt(1, 2);
7771 final double mzx = ma.getElementAt(2, 0);
7772 final double mzy = ma.getElementAt(2, 1);
7773
7774 KnownPositionAccelerometerCalibrator calibrator =
7775 new KnownPositionAccelerometerCalibrator(ba, ma);
7776
7777
7778 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7779 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7780 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7781 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7782 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7783 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7784 final Acceleration bx2 = new Acceleration(0.0,
7785 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7786 calibrator.getInitialBiasXAsAcceleration(bx2);
7787 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7788 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7789 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7790 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7791 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7792 final Acceleration by2 = new Acceleration(0.0,
7793 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7794 calibrator.getInitialBiasYAsAcceleration(by2);
7795 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7796 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7797 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7798 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7799 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7800 final Acceleration bz2 = new Acceleration(0.0,
7801 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7802 calibrator.getInitialBiasZAsAcceleration(bz2);
7803 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7804 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7805 assertEquals(calibrator.getInitialSx(), sx, 0.0);
7806 assertEquals(calibrator.getInitialSy(), sy, 0.0);
7807 assertEquals(calibrator.getInitialSz(), sz, 0.0);
7808 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
7809 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
7810 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
7811 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
7812 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
7813 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
7814 final double[] bias1 = calibrator.getInitialBias();
7815 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
7816 final double[] bias2 = new double[3];
7817 calibrator.getInitialBias(bias2);
7818 assertArrayEquals(bias1, bias2, 0.0);
7819 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7820 assertEquals(b1, ba);
7821 final Matrix b2 = new Matrix(3, 1);
7822 calibrator.getInitialBiasAsMatrix(b2);
7823 assertEquals(b1, b2);
7824 final Matrix ma1 = new Matrix(3, 3);
7825 ma1.setSubmatrix(0, 0,
7826 2, 2,
7827 new double[]{ sx, myx, mzx,
7828 mxy, sy, mzy,
7829 mxz, myz, sz});
7830 assertEquals(calibrator.getInitialMa(), ma1);
7831 final Matrix ma2 = new Matrix(3, 3);
7832 calibrator.getInitialMa(ma2);
7833 assertEquals(ma1, ma2);
7834 assertNull(calibrator.getEcefPosition());
7835 assertNull(calibrator.getNedPosition());
7836 assertFalse(calibrator.getNedPosition(null));
7837 assertNull(calibrator.getMeasurements());
7838 assertFalse(calibrator.isCommonAxisUsed());
7839 assertNull(calibrator.getListener());
7840 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7841 assertFalse(calibrator.isReady());
7842 assertFalse(calibrator.isRunning());
7843 assertNull(calibrator.getEstimatedBiases());
7844 assertFalse(calibrator.getEstimatedBiases(null));
7845 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7846 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7847 assertNull(calibrator.getEstimatedBiasFx());
7848 assertNull(calibrator.getEstimatedBiasFy());
7849 assertNull(calibrator.getEstimatedBiasFz());
7850 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7851 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7852 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7853 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7854 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7855 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7856 assertNull(calibrator.getEstimatedMa());
7857 assertNull(calibrator.getEstimatedSx());
7858 assertNull(calibrator.getEstimatedSy());
7859 assertNull(calibrator.getEstimatedSz());
7860 assertNull(calibrator.getEstimatedMxy());
7861 assertNull(calibrator.getEstimatedMxz());
7862 assertNull(calibrator.getEstimatedMyx());
7863 assertNull(calibrator.getEstimatedMyz());
7864 assertNull(calibrator.getEstimatedMzx());
7865 assertNull(calibrator.getEstimatedMzy());
7866 assertNull(calibrator.getEstimatedCovariance());
7867 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7868
7869
7870 calibrator = null;
7871 try {
7872 calibrator = new KnownPositionAccelerometerCalibrator(
7873 new Matrix(1, 1), ma);
7874 fail("IllegalArgumentException expected but not thrown");
7875 } catch (final IllegalArgumentException ignore) {
7876 }
7877 try {
7878 calibrator = new KnownPositionAccelerometerCalibrator(
7879 new Matrix(1, 3), ma);
7880 fail("IllegalArgumentException expected but not thrown");
7881 } catch (final IllegalArgumentException ignore) {
7882 }
7883 try {
7884 calibrator = new KnownPositionAccelerometerCalibrator(ba,
7885 new Matrix(1, 3));
7886 fail("IllegalArgumentException expected but not thrown");
7887 } catch (final IllegalArgumentException ignore) {
7888 }
7889 try {
7890 calibrator = new KnownPositionAccelerometerCalibrator(ba,
7891 new Matrix(3, 1));
7892 fail("IllegalArgumentException expected but not thrown");
7893 } catch (final IllegalArgumentException ignore) {
7894 }
7895 assertNull(calibrator);
7896 }
7897
7898 @Test
7899 public void testConstructor72() throws WrongSizeException {
7900 final Matrix ba = generateBa();
7901 final double biasX = ba.getElementAtIndex(0);
7902 final double biasY = ba.getElementAtIndex(1);
7903 final double biasZ = ba.getElementAtIndex(2);
7904
7905 final Matrix ma = generateMaCommonAxis();
7906 final double sx = ma.getElementAt(0, 0);
7907 final double sy = ma.getElementAt(1, 1);
7908 final double sz = ma.getElementAt(2, 2);
7909 final double mxy = ma.getElementAt(0, 1);
7910 final double mxz = ma.getElementAt(0, 2);
7911 final double myx = ma.getElementAt(1, 0);
7912 final double myz = ma.getElementAt(1, 2);
7913 final double mzx = ma.getElementAt(2, 0);
7914 final double mzy = ma.getElementAt(2, 1);
7915
7916 KnownPositionAccelerometerCalibrator calibrator =
7917 new KnownPositionAccelerometerCalibrator(ba, ma, this);
7918
7919
7920 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7921 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7922 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7923 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7924 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7925 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7926 final Acceleration bx2 = new Acceleration(0.0,
7927 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7928 calibrator.getInitialBiasXAsAcceleration(bx2);
7929 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7930 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7931 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7932 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7933 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7934 final Acceleration by2 = new Acceleration(0.0,
7935 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7936 calibrator.getInitialBiasYAsAcceleration(by2);
7937 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7938 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7939 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7940 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7941 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7942 final Acceleration bz2 = new Acceleration(0.0,
7943 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7944 calibrator.getInitialBiasZAsAcceleration(bz2);
7945 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7946 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7947 assertEquals(calibrator.getInitialSx(), sx, 0.0);
7948 assertEquals(calibrator.getInitialSy(), sy, 0.0);
7949 assertEquals(calibrator.getInitialSz(), sz, 0.0);
7950 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
7951 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
7952 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
7953 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
7954 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
7955 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
7956 final double[] bias1 = calibrator.getInitialBias();
7957 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
7958 final double[] bias2 = new double[3];
7959 calibrator.getInitialBias(bias2);
7960 assertArrayEquals(bias1, bias2, 0.0);
7961 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7962 assertEquals(b1, ba);
7963 final Matrix b2 = new Matrix(3, 1);
7964 calibrator.getInitialBiasAsMatrix(b2);
7965 assertEquals(b1, b2);
7966 final Matrix ma1 = new Matrix(3, 3);
7967 ma1.setSubmatrix(0, 0,
7968 2, 2,
7969 new double[]{ sx, myx, mzx,
7970 mxy, sy, mzy,
7971 mxz, myz, sz});
7972 assertEquals(calibrator.getInitialMa(), ma1);
7973 final Matrix ma2 = new Matrix(3, 3);
7974 calibrator.getInitialMa(ma2);
7975 assertEquals(ma1, ma2);
7976 assertNull(calibrator.getEcefPosition());
7977 assertNull(calibrator.getNedPosition());
7978 assertFalse(calibrator.getNedPosition(null));
7979 assertNull(calibrator.getMeasurements());
7980 assertFalse(calibrator.isCommonAxisUsed());
7981 assertSame(calibrator.getListener(), this);
7982 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7983 assertFalse(calibrator.isReady());
7984 assertFalse(calibrator.isRunning());
7985 assertNull(calibrator.getEstimatedBiases());
7986 assertFalse(calibrator.getEstimatedBiases(null));
7987 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7988 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7989 assertNull(calibrator.getEstimatedBiasFx());
7990 assertNull(calibrator.getEstimatedBiasFy());
7991 assertNull(calibrator.getEstimatedBiasFz());
7992 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7993 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7994 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7995 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7996 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7997 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7998 assertNull(calibrator.getEstimatedMa());
7999 assertNull(calibrator.getEstimatedSx());
8000 assertNull(calibrator.getEstimatedSy());
8001 assertNull(calibrator.getEstimatedSz());
8002 assertNull(calibrator.getEstimatedMxy());
8003 assertNull(calibrator.getEstimatedMxz());
8004 assertNull(calibrator.getEstimatedMyx());
8005 assertNull(calibrator.getEstimatedMyz());
8006 assertNull(calibrator.getEstimatedMzx());
8007 assertNull(calibrator.getEstimatedMzy());
8008 assertNull(calibrator.getEstimatedCovariance());
8009 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8010
8011
8012 calibrator = null;
8013 try {
8014 calibrator = new KnownPositionAccelerometerCalibrator(
8015 new Matrix(1, 1), ma, this);
8016 fail("IllegalArgumentException expected but not thrown");
8017 } catch (final IllegalArgumentException ignore) {
8018 }
8019 try {
8020 calibrator = new KnownPositionAccelerometerCalibrator(
8021 new Matrix(1, 3), ma, this);
8022 fail("IllegalArgumentException expected but not thrown");
8023 } catch (final IllegalArgumentException ignore) {
8024 }
8025 try {
8026 calibrator = new KnownPositionAccelerometerCalibrator(ba,
8027 new Matrix(1, 3), this);
8028 fail("IllegalArgumentException expected but not thrown");
8029 } catch (final IllegalArgumentException ignore) {
8030 }
8031 try {
8032 calibrator = new KnownPositionAccelerometerCalibrator(ba,
8033 new Matrix(3, 1), this);
8034 fail("IllegalArgumentException expected but not thrown");
8035 } catch (final IllegalArgumentException ignore) {
8036 }
8037 assertNull(calibrator);
8038 }
8039
8040 @Test
8041 public void testConstructor73() throws WrongSizeException {
8042 final Collection<StandardDeviationBodyKinematics> measurements =
8043 Collections.emptyList();
8044
8045 final Matrix ba = generateBa();
8046 final double biasX = ba.getElementAtIndex(0);
8047 final double biasY = ba.getElementAtIndex(1);
8048 final double biasZ = ba.getElementAtIndex(2);
8049
8050 final Matrix ma = generateMaCommonAxis();
8051 final double sx = ma.getElementAt(0, 0);
8052 final double sy = ma.getElementAt(1, 1);
8053 final double sz = ma.getElementAt(2, 2);
8054 final double mxy = ma.getElementAt(0, 1);
8055 final double mxz = ma.getElementAt(0, 2);
8056 final double myx = ma.getElementAt(1, 0);
8057 final double myz = ma.getElementAt(1, 2);
8058 final double mzx = ma.getElementAt(2, 0);
8059 final double mzy = ma.getElementAt(2, 1);
8060
8061 KnownPositionAccelerometerCalibrator calibrator =
8062 new KnownPositionAccelerometerCalibrator(measurements, ba, ma);
8063
8064
8065 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8066 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8067 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8068 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8069 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8070 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8071 final Acceleration bx2 = new Acceleration(0.0,
8072 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8073 calibrator.getInitialBiasXAsAcceleration(bx2);
8074 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8075 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8076 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8077 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8078 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8079 final Acceleration by2 = new Acceleration(0.0,
8080 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8081 calibrator.getInitialBiasYAsAcceleration(by2);
8082 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8083 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8084 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8085 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8086 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8087 final Acceleration bz2 = new Acceleration(0.0,
8088 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8089 calibrator.getInitialBiasZAsAcceleration(bz2);
8090 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8091 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8092 assertEquals(calibrator.getInitialSx(), sx, 0.0);
8093 assertEquals(calibrator.getInitialSy(), sy, 0.0);
8094 assertEquals(calibrator.getInitialSz(), sz, 0.0);
8095 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8096 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8097 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8098 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8099 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8100 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8101 final double[] bias1 = calibrator.getInitialBias();
8102 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
8103 final double[] bias2 = new double[3];
8104 calibrator.getInitialBias(bias2);
8105 assertArrayEquals(bias1, bias2, 0.0);
8106 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8107 assertEquals(b1, ba);
8108 final Matrix b2 = new Matrix(3, 1);
8109 calibrator.getInitialBiasAsMatrix(b2);
8110 assertEquals(b1, b2);
8111 final Matrix ma1 = new Matrix(3, 3);
8112 ma1.setSubmatrix(0, 0,
8113 2, 2,
8114 new double[]{ sx, myx, mzx,
8115 mxy, sy, mzy,
8116 mxz, myz, sz});
8117 assertEquals(calibrator.getInitialMa(), ma1);
8118 final Matrix ma2 = new Matrix(3, 3);
8119 calibrator.getInitialMa(ma2);
8120 assertEquals(ma1, ma2);
8121 assertNull(calibrator.getEcefPosition());
8122 assertNull(calibrator.getNedPosition());
8123 assertFalse(calibrator.getNedPosition(null));
8124 assertSame(calibrator.getMeasurements(), measurements);
8125 assertFalse(calibrator.isCommonAxisUsed());
8126 assertNull(calibrator.getListener());
8127 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
8128 assertFalse(calibrator.isReady());
8129 assertFalse(calibrator.isRunning());
8130 assertNull(calibrator.getEstimatedBiases());
8131 assertFalse(calibrator.getEstimatedBiases(null));
8132 assertNull(calibrator.getEstimatedBiasesAsMatrix());
8133 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8134 assertNull(calibrator.getEstimatedBiasFx());
8135 assertNull(calibrator.getEstimatedBiasFy());
8136 assertNull(calibrator.getEstimatedBiasFz());
8137 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8138 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8139 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8140 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8141 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8142 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8143 assertNull(calibrator.getEstimatedMa());
8144 assertNull(calibrator.getEstimatedSx());
8145 assertNull(calibrator.getEstimatedSy());
8146 assertNull(calibrator.getEstimatedSz());
8147 assertNull(calibrator.getEstimatedMxy());
8148 assertNull(calibrator.getEstimatedMxz());
8149 assertNull(calibrator.getEstimatedMyx());
8150 assertNull(calibrator.getEstimatedMyz());
8151 assertNull(calibrator.getEstimatedMzx());
8152 assertNull(calibrator.getEstimatedMzy());
8153 assertNull(calibrator.getEstimatedCovariance());
8154 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8155
8156
8157 calibrator = null;
8158 try {
8159 calibrator = new KnownPositionAccelerometerCalibrator(
8160 measurements, new Matrix(1, 1), ma);
8161 fail("IllegalArgumentException expected but not thrown");
8162 } catch (final IllegalArgumentException ignore) {
8163 }
8164 try {
8165 calibrator = new KnownPositionAccelerometerCalibrator(
8166 measurements, new Matrix(1, 3), ma);
8167 fail("IllegalArgumentException expected but not thrown");
8168 } catch (final IllegalArgumentException ignore) {
8169 }
8170 try {
8171 calibrator = new KnownPositionAccelerometerCalibrator(
8172 measurements, ba, new Matrix(1, 3));
8173 fail("IllegalArgumentException expected but not thrown");
8174 } catch (final IllegalArgumentException ignore) {
8175 }
8176 try {
8177 calibrator = new KnownPositionAccelerometerCalibrator(
8178 measurements, ba, new Matrix(3, 1));
8179 fail("IllegalArgumentException expected but not thrown");
8180 } catch (final IllegalArgumentException ignore) {
8181 }
8182 assertNull(calibrator);
8183 }
8184
8185 @Test
8186 public void testConstructor74() throws WrongSizeException {
8187 final Collection<StandardDeviationBodyKinematics> measurements =
8188 Collections.emptyList();
8189
8190 final Matrix ba = generateBa();
8191 final double biasX = ba.getElementAtIndex(0);
8192 final double biasY = ba.getElementAtIndex(1);
8193 final double biasZ = ba.getElementAtIndex(2);
8194
8195 final Matrix ma = generateMaCommonAxis();
8196 final double sx = ma.getElementAt(0, 0);
8197 final double sy = ma.getElementAt(1, 1);
8198 final double sz = ma.getElementAt(2, 2);
8199 final double mxy = ma.getElementAt(0, 1);
8200 final double mxz = ma.getElementAt(0, 2);
8201 final double myx = ma.getElementAt(1, 0);
8202 final double myz = ma.getElementAt(1, 2);
8203 final double mzx = ma.getElementAt(2, 0);
8204 final double mzy = ma.getElementAt(2, 1);
8205
8206 KnownPositionAccelerometerCalibrator calibrator =
8207 new KnownPositionAccelerometerCalibrator(measurements, ba, ma,
8208 this);
8209
8210
8211 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8212 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8213 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8214 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8215 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8216 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8217 final Acceleration bx2 = new Acceleration(0.0,
8218 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8219 calibrator.getInitialBiasXAsAcceleration(bx2);
8220 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8221 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8222 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8223 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8224 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8225 final Acceleration by2 = new Acceleration(0.0,
8226 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8227 calibrator.getInitialBiasYAsAcceleration(by2);
8228 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8229 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8230 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8231 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8232 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8233 final Acceleration bz2 = new Acceleration(0.0,
8234 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8235 calibrator.getInitialBiasZAsAcceleration(bz2);
8236 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8237 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8238 assertEquals(calibrator.getInitialSx(), sx, 0.0);
8239 assertEquals(calibrator.getInitialSy(), sy, 0.0);
8240 assertEquals(calibrator.getInitialSz(), sz, 0.0);
8241 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8242 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8243 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8244 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8245 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8246 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8247 final double[] bias1 = calibrator.getInitialBias();
8248 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
8249 final double[] bias2 = new double[3];
8250 calibrator.getInitialBias(bias2);
8251 assertArrayEquals(bias1, bias2, 0.0);
8252 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8253 assertEquals(b1, ba);
8254 final Matrix b2 = new Matrix(3, 1);
8255 calibrator.getInitialBiasAsMatrix(b2);
8256 assertEquals(b1, b2);
8257 final Matrix ma1 = new Matrix(3, 3);
8258 ma1.setSubmatrix(0, 0,
8259 2, 2,
8260 new double[]{ sx, myx, mzx,
8261 mxy, sy, mzy,
8262 mxz, myz, sz});
8263 assertEquals(calibrator.getInitialMa(), ma1);
8264 final Matrix ma2 = new Matrix(3, 3);
8265 calibrator.getInitialMa(ma2);
8266 assertEquals(ma1, ma2);
8267 assertNull(calibrator.getEcefPosition());
8268 assertNull(calibrator.getNedPosition());
8269 assertFalse(calibrator.getNedPosition(null));
8270 assertSame(calibrator.getMeasurements(), measurements);
8271 assertFalse(calibrator.isCommonAxisUsed());
8272 assertSame(calibrator.getListener(), this);
8273 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
8274 assertFalse(calibrator.isReady());
8275 assertFalse(calibrator.isRunning());
8276 assertNull(calibrator.getEstimatedBiases());
8277 assertFalse(calibrator.getEstimatedBiases(null));
8278 assertNull(calibrator.getEstimatedBiasesAsMatrix());
8279 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8280 assertNull(calibrator.getEstimatedBiasFx());
8281 assertNull(calibrator.getEstimatedBiasFy());
8282 assertNull(calibrator.getEstimatedBiasFz());
8283 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8284 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8285 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8286 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8287 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8288 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8289 assertNull(calibrator.getEstimatedMa());
8290 assertNull(calibrator.getEstimatedSx());
8291 assertNull(calibrator.getEstimatedSy());
8292 assertNull(calibrator.getEstimatedSz());
8293 assertNull(calibrator.getEstimatedMxy());
8294 assertNull(calibrator.getEstimatedMxz());
8295 assertNull(calibrator.getEstimatedMyx());
8296 assertNull(calibrator.getEstimatedMyz());
8297 assertNull(calibrator.getEstimatedMzx());
8298 assertNull(calibrator.getEstimatedMzy());
8299 assertNull(calibrator.getEstimatedCovariance());
8300 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8301
8302
8303 calibrator = null;
8304 try {
8305 calibrator = new KnownPositionAccelerometerCalibrator(
8306 measurements, new Matrix(1, 1), ma, this);
8307 fail("IllegalArgumentException expected but not thrown");
8308 } catch (final IllegalArgumentException ignore) {
8309 }
8310 try {
8311 calibrator = new KnownPositionAccelerometerCalibrator(
8312 measurements, new Matrix(1, 3), ma, this);
8313 fail("IllegalArgumentException expected but not thrown");
8314 } catch (final IllegalArgumentException ignore) {
8315 }
8316 try {
8317 calibrator = new KnownPositionAccelerometerCalibrator(
8318 measurements, ba, new Matrix(1, 3), this);
8319 fail("IllegalArgumentException expected but not thrown");
8320 } catch (final IllegalArgumentException ignore) {
8321 }
8322 try {
8323 calibrator = new KnownPositionAccelerometerCalibrator(
8324 measurements, ba, new Matrix(3, 1), this);
8325 fail("IllegalArgumentException expected but not thrown");
8326 } catch (final IllegalArgumentException ignore) {
8327 }
8328 assertNull(calibrator);
8329 }
8330
8331 @Test
8332 public void testConstructor75() throws WrongSizeException {
8333 final Matrix ba = generateBa();
8334 final double biasX = ba.getElementAtIndex(0);
8335 final double biasY = ba.getElementAtIndex(1);
8336 final double biasZ = ba.getElementAtIndex(2);
8337
8338 final Matrix ma = generateMaCommonAxis();
8339 final double sx = ma.getElementAt(0, 0);
8340 final double sy = ma.getElementAt(1, 1);
8341 final double sz = ma.getElementAt(2, 2);
8342 final double mxy = ma.getElementAt(0, 1);
8343 final double mxz = ma.getElementAt(0, 2);
8344 final double myx = ma.getElementAt(1, 0);
8345 final double myz = ma.getElementAt(1, 2);
8346 final double mzx = ma.getElementAt(2, 0);
8347 final double mzy = ma.getElementAt(2, 1);
8348
8349 KnownPositionAccelerometerCalibrator calibrator =
8350 new KnownPositionAccelerometerCalibrator(true, ba, ma);
8351
8352
8353 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8354 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8355 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8356 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8357 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8358 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8359 final Acceleration bx2 = new Acceleration(0.0,
8360 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8361 calibrator.getInitialBiasXAsAcceleration(bx2);
8362 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8363 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8364 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8365 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8366 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8367 final Acceleration by2 = new Acceleration(0.0,
8368 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8369 calibrator.getInitialBiasYAsAcceleration(by2);
8370 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8371 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8372 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8373 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8374 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8375 final Acceleration bz2 = new Acceleration(0.0,
8376 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8377 calibrator.getInitialBiasZAsAcceleration(bz2);
8378 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8379 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8380 assertEquals(calibrator.getInitialSx(), sx, 0.0);
8381 assertEquals(calibrator.getInitialSy(), sy, 0.0);
8382 assertEquals(calibrator.getInitialSz(), sz, 0.0);
8383 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8384 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8385 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8386 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8387 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8388 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8389 final double[] bias1 = calibrator.getInitialBias();
8390 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
8391 final double[] bias2 = new double[3];
8392 calibrator.getInitialBias(bias2);
8393 assertArrayEquals(bias1, bias2, 0.0);
8394 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8395 assertEquals(b1, ba);
8396 final Matrix b2 = new Matrix(3, 1);
8397 calibrator.getInitialBiasAsMatrix(b2);
8398 assertEquals(b1, b2);
8399 final Matrix ma1 = new Matrix(3, 3);
8400 ma1.setSubmatrix(0, 0,
8401 2, 2,
8402 new double[]{ sx, myx, mzx,
8403 mxy, sy, mzy,
8404 mxz, myz, sz});
8405 assertEquals(calibrator.getInitialMa(), ma1);
8406 final Matrix ma2 = new Matrix(3, 3);
8407 calibrator.getInitialMa(ma2);
8408 assertEquals(ma1, ma2);
8409 assertNull(calibrator.getEcefPosition());
8410 assertNull(calibrator.getNedPosition());
8411 assertFalse(calibrator.getNedPosition(null));
8412 assertNull(calibrator.getMeasurements());
8413 assertTrue(calibrator.isCommonAxisUsed());
8414 assertNull(calibrator.getListener());
8415 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8416 assertFalse(calibrator.isReady());
8417 assertFalse(calibrator.isRunning());
8418 assertNull(calibrator.getEstimatedBiases());
8419 assertFalse(calibrator.getEstimatedBiases(null));
8420 assertNull(calibrator.getEstimatedBiasesAsMatrix());
8421 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8422 assertNull(calibrator.getEstimatedBiasFx());
8423 assertNull(calibrator.getEstimatedBiasFy());
8424 assertNull(calibrator.getEstimatedBiasFz());
8425 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8426 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8427 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8428 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8429 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8430 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8431 assertNull(calibrator.getEstimatedMa());
8432 assertNull(calibrator.getEstimatedSx());
8433 assertNull(calibrator.getEstimatedSy());
8434 assertNull(calibrator.getEstimatedSz());
8435 assertNull(calibrator.getEstimatedMxy());
8436 assertNull(calibrator.getEstimatedMxz());
8437 assertNull(calibrator.getEstimatedMyx());
8438 assertNull(calibrator.getEstimatedMyz());
8439 assertNull(calibrator.getEstimatedMzx());
8440 assertNull(calibrator.getEstimatedMzy());
8441 assertNull(calibrator.getEstimatedCovariance());
8442 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8443
8444
8445 calibrator = null;
8446 try {
8447 calibrator = new KnownPositionAccelerometerCalibrator(
8448 true, new Matrix(1, 1), ma);
8449 fail("IllegalArgumentException expected but not thrown");
8450 } catch (final IllegalArgumentException ignore) {
8451 }
8452 try {
8453 calibrator = new KnownPositionAccelerometerCalibrator(
8454 true, new Matrix(1, 3), ma);
8455 fail("IllegalArgumentException expected but not thrown");
8456 } catch (final IllegalArgumentException ignore) {
8457 }
8458 try {
8459 calibrator = new KnownPositionAccelerometerCalibrator(
8460 true, ba, new Matrix(1, 3));
8461 fail("IllegalArgumentException expected but not thrown");
8462 } catch (final IllegalArgumentException ignore) {
8463 }
8464 try {
8465 calibrator = new KnownPositionAccelerometerCalibrator(
8466 true, ba, new Matrix(3, 1));
8467 fail("IllegalArgumentException expected but not thrown");
8468 } catch (final IllegalArgumentException ignore) {
8469 }
8470 assertNull(calibrator);
8471 }
8472
8473 @Test
8474 public void testConstructor76() throws WrongSizeException {
8475 final Matrix ba = generateBa();
8476 final double biasX = ba.getElementAtIndex(0);
8477 final double biasY = ba.getElementAtIndex(1);
8478 final double biasZ = ba.getElementAtIndex(2);
8479
8480 final Matrix ma = generateMaCommonAxis();
8481 final double sx = ma.getElementAt(0, 0);
8482 final double sy = ma.getElementAt(1, 1);
8483 final double sz = ma.getElementAt(2, 2);
8484 final double mxy = ma.getElementAt(0, 1);
8485 final double mxz = ma.getElementAt(0, 2);
8486 final double myx = ma.getElementAt(1, 0);
8487 final double myz = ma.getElementAt(1, 2);
8488 final double mzx = ma.getElementAt(2, 0);
8489 final double mzy = ma.getElementAt(2, 1);
8490
8491 KnownPositionAccelerometerCalibrator calibrator =
8492 new KnownPositionAccelerometerCalibrator(true, ba, ma,
8493 this);
8494
8495
8496 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8497 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8498 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8499 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8500 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8501 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8502 final Acceleration bx2 = new Acceleration(0.0,
8503 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8504 calibrator.getInitialBiasXAsAcceleration(bx2);
8505 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8506 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8507 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8508 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8509 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8510 final Acceleration by2 = new Acceleration(0.0,
8511 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8512 calibrator.getInitialBiasYAsAcceleration(by2);
8513 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8514 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8515 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8516 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8517 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8518 final Acceleration bz2 = new Acceleration(0.0,
8519 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8520 calibrator.getInitialBiasZAsAcceleration(bz2);
8521 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8522 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8523 assertEquals(calibrator.getInitialSx(), sx, 0.0);
8524 assertEquals(calibrator.getInitialSy(), sy, 0.0);
8525 assertEquals(calibrator.getInitialSz(), sz, 0.0);
8526 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8527 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8528 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8529 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8530 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8531 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8532 final double[] bias1 = calibrator.getInitialBias();
8533 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
8534 final double[] bias2 = new double[3];
8535 calibrator.getInitialBias(bias2);
8536 assertArrayEquals(bias1, bias2, 0.0);
8537 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8538 assertEquals(b1, ba);
8539 final Matrix b2 = new Matrix(3, 1);
8540 calibrator.getInitialBiasAsMatrix(b2);
8541 assertEquals(b1, b2);
8542 final Matrix ma1 = new Matrix(3, 3);
8543 ma1.setSubmatrix(0, 0,
8544 2, 2,
8545 new double[]{ sx, myx, mzx,
8546 mxy, sy, mzy,
8547 mxz, myz, sz});
8548 assertEquals(calibrator.getInitialMa(), ma1);
8549 final Matrix ma2 = new Matrix(3, 3);
8550 calibrator.getInitialMa(ma2);
8551 assertEquals(ma1, ma2);
8552 assertNull(calibrator.getEcefPosition());
8553 assertNull(calibrator.getNedPosition());
8554 assertFalse(calibrator.getNedPosition(null));
8555 assertNull(calibrator.getMeasurements());
8556 assertTrue(calibrator.isCommonAxisUsed());
8557 assertSame(calibrator.getListener(), this);
8558 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8559 assertFalse(calibrator.isReady());
8560 assertFalse(calibrator.isRunning());
8561 assertNull(calibrator.getEstimatedBiases());
8562 assertFalse(calibrator.getEstimatedBiases(null));
8563 assertNull(calibrator.getEstimatedBiasesAsMatrix());
8564 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8565 assertNull(calibrator.getEstimatedBiasFx());
8566 assertNull(calibrator.getEstimatedBiasFy());
8567 assertNull(calibrator.getEstimatedBiasFz());
8568 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8569 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8570 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8571 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8572 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8573 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8574 assertNull(calibrator.getEstimatedMa());
8575 assertNull(calibrator.getEstimatedSx());
8576 assertNull(calibrator.getEstimatedSy());
8577 assertNull(calibrator.getEstimatedSz());
8578 assertNull(calibrator.getEstimatedMxy());
8579 assertNull(calibrator.getEstimatedMxz());
8580 assertNull(calibrator.getEstimatedMyx());
8581 assertNull(calibrator.getEstimatedMyz());
8582 assertNull(calibrator.getEstimatedMzx());
8583 assertNull(calibrator.getEstimatedMzy());
8584 assertNull(calibrator.getEstimatedCovariance());
8585 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8586
8587
8588 calibrator = null;
8589 try {
8590 calibrator = new KnownPositionAccelerometerCalibrator(
8591 true, new Matrix(1, 1), ma,
8592 this);
8593 fail("IllegalArgumentException expected but not thrown");
8594 } catch (final IllegalArgumentException ignore) {
8595 }
8596 try {
8597 calibrator = new KnownPositionAccelerometerCalibrator(
8598 true, new Matrix(1, 3), ma,
8599 this);
8600 fail("IllegalArgumentException expected but not thrown");
8601 } catch (final IllegalArgumentException ignore) {
8602 }
8603 try {
8604 calibrator = new KnownPositionAccelerometerCalibrator(
8605 true, ba, new Matrix(1, 3),
8606 this);
8607 fail("IllegalArgumentException expected but not thrown");
8608 } catch (final IllegalArgumentException ignore) {
8609 }
8610 try {
8611 calibrator = new KnownPositionAccelerometerCalibrator(
8612 true, ba, new Matrix(3, 1),
8613 this);
8614 fail("IllegalArgumentException expected but not thrown");
8615 } catch (final IllegalArgumentException ignore) {
8616 }
8617 assertNull(calibrator);
8618 }
8619
8620 @Test
8621 public void testConstructor77() throws WrongSizeException {
8622 final Collection<StandardDeviationBodyKinematics> measurements =
8623 Collections.emptyList();
8624
8625 final Matrix ba = generateBa();
8626 final double biasX = ba.getElementAtIndex(0);
8627 final double biasY = ba.getElementAtIndex(1);
8628 final double biasZ = ba.getElementAtIndex(2);
8629
8630 final Matrix ma = generateMaCommonAxis();
8631 final double sx = ma.getElementAt(0, 0);
8632 final double sy = ma.getElementAt(1, 1);
8633 final double sz = ma.getElementAt(2, 2);
8634 final double mxy = ma.getElementAt(0, 1);
8635 final double mxz = ma.getElementAt(0, 2);
8636 final double myx = ma.getElementAt(1, 0);
8637 final double myz = ma.getElementAt(1, 2);
8638 final double mzx = ma.getElementAt(2, 0);
8639 final double mzy = ma.getElementAt(2, 1);
8640
8641 KnownPositionAccelerometerCalibrator calibrator =
8642 new KnownPositionAccelerometerCalibrator(measurements,
8643 true, ba, ma);
8644
8645
8646 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8647 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8648 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8649 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8650 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8651 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8652 final Acceleration bx2 = new Acceleration(0.0,
8653 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8654 calibrator.getInitialBiasXAsAcceleration(bx2);
8655 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8656 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8657 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8658 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8659 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8660 final Acceleration by2 = new Acceleration(0.0,
8661 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8662 calibrator.getInitialBiasYAsAcceleration(by2);
8663 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8664 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8665 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8666 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8667 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8668 final Acceleration bz2 = new Acceleration(0.0,
8669 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8670 calibrator.getInitialBiasZAsAcceleration(bz2);
8671 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8672 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8673 assertEquals(calibrator.getInitialSx(), sx, 0.0);
8674 assertEquals(calibrator.getInitialSy(), sy, 0.0);
8675 assertEquals(calibrator.getInitialSz(), sz, 0.0);
8676 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8677 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8678 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8679 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8680 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8681 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8682 final double[] bias1 = calibrator.getInitialBias();
8683 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
8684 final double[] bias2 = new double[3];
8685 calibrator.getInitialBias(bias2);
8686 assertArrayEquals(bias1, bias2, 0.0);
8687 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8688 assertEquals(b1, ba);
8689 final Matrix b2 = new Matrix(3, 1);
8690 calibrator.getInitialBiasAsMatrix(b2);
8691 assertEquals(b1, b2);
8692 final Matrix ma1 = new Matrix(3, 3);
8693 ma1.setSubmatrix(0, 0,
8694 2, 2,
8695 new double[]{ sx, myx, mzx,
8696 mxy, sy, mzy,
8697 mxz, myz, sz});
8698 assertEquals(calibrator.getInitialMa(), ma1);
8699 final Matrix ma2 = new Matrix(3, 3);
8700 calibrator.getInitialMa(ma2);
8701 assertEquals(ma1, ma2);
8702 assertNull(calibrator.getEcefPosition());
8703 assertNull(calibrator.getNedPosition());
8704 assertFalse(calibrator.getNedPosition(null));
8705 assertSame(calibrator.getMeasurements(), measurements);
8706 assertTrue(calibrator.isCommonAxisUsed());
8707 assertNull(calibrator.getListener());
8708 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8709 assertFalse(calibrator.isReady());
8710 assertFalse(calibrator.isRunning());
8711 assertNull(calibrator.getEstimatedBiases());
8712 assertFalse(calibrator.getEstimatedBiases(null));
8713 assertNull(calibrator.getEstimatedBiasesAsMatrix());
8714 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8715 assertNull(calibrator.getEstimatedBiasFx());
8716 assertNull(calibrator.getEstimatedBiasFy());
8717 assertNull(calibrator.getEstimatedBiasFz());
8718 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8719 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8720 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8721 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8722 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8723 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8724 assertNull(calibrator.getEstimatedMa());
8725 assertNull(calibrator.getEstimatedSx());
8726 assertNull(calibrator.getEstimatedSy());
8727 assertNull(calibrator.getEstimatedSz());
8728 assertNull(calibrator.getEstimatedMxy());
8729 assertNull(calibrator.getEstimatedMxz());
8730 assertNull(calibrator.getEstimatedMyx());
8731 assertNull(calibrator.getEstimatedMyz());
8732 assertNull(calibrator.getEstimatedMzx());
8733 assertNull(calibrator.getEstimatedMzy());
8734 assertNull(calibrator.getEstimatedCovariance());
8735 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8736
8737
8738 calibrator = null;
8739 try {
8740 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
8741 true, new Matrix(1, 1), ma);
8742 fail("IllegalArgumentException expected but not thrown");
8743 } catch (final IllegalArgumentException ignore) {
8744 }
8745 try {
8746 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
8747 true, new Matrix(1, 3), ma);
8748 fail("IllegalArgumentException expected but not thrown");
8749 } catch (final IllegalArgumentException ignore) {
8750 }
8751 try {
8752 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
8753 true, ba, new Matrix(1, 3));
8754 fail("IllegalArgumentException expected but not thrown");
8755 } catch (final IllegalArgumentException ignore) {
8756 }
8757 try {
8758 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
8759 true, ba, new Matrix(3, 1));
8760 fail("IllegalArgumentException expected but not thrown");
8761 } catch (final IllegalArgumentException ignore) {
8762 }
8763 assertNull(calibrator);
8764 }
8765
8766 @Test
8767 public void testConstructor78() throws WrongSizeException {
8768 final Collection<StandardDeviationBodyKinematics> measurements =
8769 Collections.emptyList();
8770
8771 final Matrix ba = generateBa();
8772 final double biasX = ba.getElementAtIndex(0);
8773 final double biasY = ba.getElementAtIndex(1);
8774 final double biasZ = ba.getElementAtIndex(2);
8775
8776 final Matrix ma = generateMaCommonAxis();
8777 final double sx = ma.getElementAt(0, 0);
8778 final double sy = ma.getElementAt(1, 1);
8779 final double sz = ma.getElementAt(2, 2);
8780 final double mxy = ma.getElementAt(0, 1);
8781 final double mxz = ma.getElementAt(0, 2);
8782 final double myx = ma.getElementAt(1, 0);
8783 final double myz = ma.getElementAt(1, 2);
8784 final double mzx = ma.getElementAt(2, 0);
8785 final double mzy = ma.getElementAt(2, 1);
8786
8787 KnownPositionAccelerometerCalibrator calibrator =
8788 new KnownPositionAccelerometerCalibrator(measurements,
8789 true, ba, ma, this);
8790
8791
8792 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8793 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8794 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8795 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8796 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8797 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8798 final Acceleration bx2 = new Acceleration(0.0,
8799 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8800 calibrator.getInitialBiasXAsAcceleration(bx2);
8801 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8802 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8803 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8804 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8805 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8806 final Acceleration by2 = new Acceleration(0.0,
8807 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8808 calibrator.getInitialBiasYAsAcceleration(by2);
8809 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8810 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8811 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8812 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8813 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8814 final Acceleration bz2 = new Acceleration(0.0,
8815 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8816 calibrator.getInitialBiasZAsAcceleration(bz2);
8817 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8818 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8819 assertEquals(calibrator.getInitialSx(), sx, 0.0);
8820 assertEquals(calibrator.getInitialSy(), sy, 0.0);
8821 assertEquals(calibrator.getInitialSz(), sz, 0.0);
8822 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8823 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8824 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8825 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8826 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8827 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8828 final double[] bias1 = calibrator.getInitialBias();
8829 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
8830 final double[] bias2 = new double[3];
8831 calibrator.getInitialBias(bias2);
8832 assertArrayEquals(bias1, bias2, 0.0);
8833 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8834 assertEquals(b1, ba);
8835 final Matrix b2 = new Matrix(3, 1);
8836 calibrator.getInitialBiasAsMatrix(b2);
8837 assertEquals(b1, b2);
8838 final Matrix ma1 = new Matrix(3, 3);
8839 ma1.setSubmatrix(0, 0,
8840 2, 2,
8841 new double[]{ sx, myx, mzx,
8842 mxy, sy, mzy,
8843 mxz, myz, sz});
8844 assertEquals(calibrator.getInitialMa(), ma1);
8845 final Matrix ma2 = new Matrix(3, 3);
8846 calibrator.getInitialMa(ma2);
8847 assertEquals(ma1, ma2);
8848 assertNull(calibrator.getEcefPosition());
8849 assertNull(calibrator.getNedPosition());
8850 assertFalse(calibrator.getNedPosition(null));
8851 assertSame(calibrator.getMeasurements(), measurements);
8852 assertTrue(calibrator.isCommonAxisUsed());
8853 assertSame(calibrator.getListener(), this);
8854 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8855 assertFalse(calibrator.isReady());
8856 assertFalse(calibrator.isRunning());
8857 assertNull(calibrator.getEstimatedBiases());
8858 assertFalse(calibrator.getEstimatedBiases(null));
8859 assertNull(calibrator.getEstimatedBiasesAsMatrix());
8860 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8861 assertNull(calibrator.getEstimatedBiasFx());
8862 assertNull(calibrator.getEstimatedBiasFy());
8863 assertNull(calibrator.getEstimatedBiasFz());
8864 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8865 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8866 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8867 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8868 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8869 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8870 assertNull(calibrator.getEstimatedMa());
8871 assertNull(calibrator.getEstimatedSx());
8872 assertNull(calibrator.getEstimatedSy());
8873 assertNull(calibrator.getEstimatedSz());
8874 assertNull(calibrator.getEstimatedMxy());
8875 assertNull(calibrator.getEstimatedMxz());
8876 assertNull(calibrator.getEstimatedMyx());
8877 assertNull(calibrator.getEstimatedMyz());
8878 assertNull(calibrator.getEstimatedMzx());
8879 assertNull(calibrator.getEstimatedMzy());
8880 assertNull(calibrator.getEstimatedCovariance());
8881 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8882
8883
8884 calibrator = null;
8885 try {
8886 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
8887 true, new Matrix(1, 1), ma,
8888 this);
8889 fail("IllegalArgumentException expected but not thrown");
8890 } catch (final IllegalArgumentException ignore) {
8891 }
8892 try {
8893 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
8894 true, new Matrix(1, 3), ma,
8895 this);
8896 fail("IllegalArgumentException expected but not thrown");
8897 } catch (final IllegalArgumentException ignore) {
8898 }
8899 try {
8900 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
8901 true, ba, new Matrix(1, 3),
8902 this);
8903 fail("IllegalArgumentException expected but not thrown");
8904 } catch (final IllegalArgumentException ignore) {
8905 }
8906 try {
8907 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
8908 true, ba, new Matrix(3, 1),
8909 this);
8910 fail("IllegalArgumentException expected but not thrown");
8911 } catch (final IllegalArgumentException ignore) {
8912 }
8913 assertNull(calibrator);
8914 }
8915
8916 @Test
8917 public void testConstructor79() throws WrongSizeException {
8918 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
8919 final double latitude = Math.toRadians(
8920 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
8921 final double longitude = Math.toRadians(
8922 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8923 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8924 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
8925 final NEDVelocity nedVelocity = new NEDVelocity();
8926 final ECEFPosition ecefPosition = new ECEFPosition();
8927 final ECEFVelocity ecefVelocity = new ECEFVelocity();
8928 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
8929 ecefPosition, ecefVelocity);
8930
8931 final KnownPositionAccelerometerCalibrator calibrator =
8932 new KnownPositionAccelerometerCalibrator(ecefPosition);
8933
8934
8935 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
8936 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
8937 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
8938 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8939 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
8940 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8941 final Acceleration bx2 = new Acceleration(0.0,
8942 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8943 calibrator.getInitialBiasXAsAcceleration(bx2);
8944 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
8945 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8946 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8947 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
8948 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8949 final Acceleration by2 = new Acceleration(0.0,
8950 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8951 calibrator.getInitialBiasYAsAcceleration(by2);
8952 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
8953 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8954 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8955 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
8956 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8957 final Acceleration bz2 = new Acceleration(0.0,
8958 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8959 calibrator.getInitialBiasZAsAcceleration(bz2);
8960 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
8961 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8962 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
8963 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
8964 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
8965 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
8966 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
8967 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
8968 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
8969 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
8970 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
8971 final double[] bias1 = calibrator.getInitialBias();
8972 assertArrayEquals(bias1, new double[3], 0.0);
8973 final double[] bias2 = new double[3];
8974 calibrator.getInitialBias(bias2);
8975 assertArrayEquals(bias1, bias2, 0.0);
8976 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8977 assertEquals(b1, new Matrix(3, 1));
8978 final Matrix b2 = new Matrix(3, 1);
8979 calibrator.getInitialBiasAsMatrix(b2);
8980 assertEquals(b1, b2);
8981 final Matrix ma1 = calibrator.getInitialMa();
8982 assertEquals(ma1, new Matrix(3, 3));
8983 final Matrix ma2 = new Matrix(3, 3);
8984 calibrator.getInitialMa(ma2);
8985 assertEquals(ma1, ma2);
8986 assertSame(calibrator.getEcefPosition(), ecefPosition);
8987 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
8988 final NEDPosition nedPosition1 = new NEDPosition();
8989 assertTrue(calibrator.getNedPosition(nedPosition1));
8990 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
8991 assertNull(calibrator.getMeasurements());
8992 assertFalse(calibrator.isCommonAxisUsed());
8993 assertNull(calibrator.getListener());
8994 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
8995 assertFalse(calibrator.isReady());
8996 assertFalse(calibrator.isRunning());
8997 assertNull(calibrator.getEstimatedBiases());
8998 assertFalse(calibrator.getEstimatedBiases(null));
8999 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9000 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9001 assertNull(calibrator.getEstimatedBiasFx());
9002 assertNull(calibrator.getEstimatedBiasFy());
9003 assertNull(calibrator.getEstimatedBiasFz());
9004 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9005 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9006 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9007 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9008 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9009 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9010 assertNull(calibrator.getEstimatedMa());
9011 assertNull(calibrator.getEstimatedSx());
9012 assertNull(calibrator.getEstimatedSy());
9013 assertNull(calibrator.getEstimatedSz());
9014 assertNull(calibrator.getEstimatedMxy());
9015 assertNull(calibrator.getEstimatedMxz());
9016 assertNull(calibrator.getEstimatedMyx());
9017 assertNull(calibrator.getEstimatedMyz());
9018 assertNull(calibrator.getEstimatedMzx());
9019 assertNull(calibrator.getEstimatedMzy());
9020 assertNull(calibrator.getEstimatedCovariance());
9021 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9022 }
9023
9024 @Test
9025 public void testConstructor80() throws WrongSizeException {
9026 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9027 final double latitude = Math.toRadians(
9028 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9029 final double longitude = Math.toRadians(
9030 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9031 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9032 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9033 final NEDVelocity nedVelocity = new NEDVelocity();
9034 final ECEFPosition ecefPosition = new ECEFPosition();
9035 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9036 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9037 ecefPosition, ecefVelocity);
9038
9039 final KnownPositionAccelerometerCalibrator calibrator =
9040 new KnownPositionAccelerometerCalibrator(ecefPosition, this);
9041
9042
9043 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9044 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9045 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9046 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9047 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9048 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9049 final Acceleration bx2 = new Acceleration(0.0,
9050 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9051 calibrator.getInitialBiasXAsAcceleration(bx2);
9052 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9053 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9054 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9055 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9056 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9057 final Acceleration by2 = new Acceleration(0.0,
9058 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9059 calibrator.getInitialBiasYAsAcceleration(by2);
9060 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9061 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9062 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9063 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9064 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9065 final Acceleration bz2 = new Acceleration(0.0,
9066 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9067 calibrator.getInitialBiasZAsAcceleration(bz2);
9068 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9069 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9070 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9071 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9072 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9073 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9074 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9075 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9076 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9077 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9078 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9079 final double[] bias1 = calibrator.getInitialBias();
9080 assertArrayEquals(bias1, new double[3], 0.0);
9081 final double[] bias2 = new double[3];
9082 calibrator.getInitialBias(bias2);
9083 assertArrayEquals(bias1, bias2, 0.0);
9084 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9085 assertEquals(b1, new Matrix(3, 1));
9086 final Matrix b2 = new Matrix(3, 1);
9087 calibrator.getInitialBiasAsMatrix(b2);
9088 assertEquals(b1, b2);
9089 final Matrix ma1 = calibrator.getInitialMa();
9090 assertEquals(ma1, new Matrix(3, 3));
9091 final Matrix ma2 = new Matrix(3, 3);
9092 calibrator.getInitialMa(ma2);
9093 assertEquals(ma1, ma2);
9094 assertSame(calibrator.getEcefPosition(), ecefPosition);
9095 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9096 final NEDPosition nedPosition1 = new NEDPosition();
9097 assertTrue(calibrator.getNedPosition(nedPosition1));
9098 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9099 assertNull(calibrator.getMeasurements());
9100 assertFalse(calibrator.isCommonAxisUsed());
9101 assertSame(calibrator.getListener(), this);
9102 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9103 assertFalse(calibrator.isReady());
9104 assertFalse(calibrator.isRunning());
9105 assertNull(calibrator.getEstimatedBiases());
9106 assertFalse(calibrator.getEstimatedBiases(null));
9107 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9108 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9109 assertNull(calibrator.getEstimatedBiasFx());
9110 assertNull(calibrator.getEstimatedBiasFy());
9111 assertNull(calibrator.getEstimatedBiasFz());
9112 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9113 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9114 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9115 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9116 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9117 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9118 assertNull(calibrator.getEstimatedMa());
9119 assertNull(calibrator.getEstimatedSx());
9120 assertNull(calibrator.getEstimatedSy());
9121 assertNull(calibrator.getEstimatedSz());
9122 assertNull(calibrator.getEstimatedMxy());
9123 assertNull(calibrator.getEstimatedMxz());
9124 assertNull(calibrator.getEstimatedMyx());
9125 assertNull(calibrator.getEstimatedMyz());
9126 assertNull(calibrator.getEstimatedMzx());
9127 assertNull(calibrator.getEstimatedMzy());
9128 assertNull(calibrator.getEstimatedCovariance());
9129 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9130 }
9131
9132 @Test
9133 public void testConstructor81() throws WrongSizeException {
9134 final Collection<StandardDeviationBodyKinematics> measurements =
9135 Collections.emptyList();
9136
9137 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9138 final double latitude = Math.toRadians(
9139 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9140 final double longitude = Math.toRadians(
9141 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9142 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9143 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9144 final NEDVelocity nedVelocity = new NEDVelocity();
9145 final ECEFPosition ecefPosition = new ECEFPosition();
9146 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9147 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9148 ecefPosition, ecefVelocity);
9149
9150 final KnownPositionAccelerometerCalibrator calibrator =
9151 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements);
9152
9153
9154 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9155 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9156 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9157 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9158 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9159 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9160 final Acceleration bx2 = new Acceleration(0.0,
9161 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9162 calibrator.getInitialBiasXAsAcceleration(bx2);
9163 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9164 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9165 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9166 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9167 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9168 final Acceleration by2 = new Acceleration(0.0,
9169 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9170 calibrator.getInitialBiasYAsAcceleration(by2);
9171 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9172 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9173 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9174 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9175 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9176 final Acceleration bz2 = new Acceleration(0.0,
9177 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9178 calibrator.getInitialBiasZAsAcceleration(bz2);
9179 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9180 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9181 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9182 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9183 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9184 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9185 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9186 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9187 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9188 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9189 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9190 final double[] bias1 = calibrator.getInitialBias();
9191 assertArrayEquals(bias1, new double[3], 0.0);
9192 final double[] bias2 = new double[3];
9193 calibrator.getInitialBias(bias2);
9194 assertArrayEquals(bias1, bias2, 0.0);
9195 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9196 assertEquals(b1, new Matrix(3, 1));
9197 final Matrix b2 = new Matrix(3, 1);
9198 calibrator.getInitialBiasAsMatrix(b2);
9199 assertEquals(b1, b2);
9200 final Matrix ma1 = calibrator.getInitialMa();
9201 assertEquals(ma1, new Matrix(3, 3));
9202 final Matrix ma2 = new Matrix(3, 3);
9203 calibrator.getInitialMa(ma2);
9204 assertEquals(ma1, ma2);
9205 assertSame(calibrator.getEcefPosition(), ecefPosition);
9206 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9207 final NEDPosition nedPosition1 = new NEDPosition();
9208 assertTrue(calibrator.getNedPosition(nedPosition1));
9209 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9210 assertSame(calibrator.getMeasurements(), measurements);
9211 assertFalse(calibrator.isCommonAxisUsed());
9212 assertNull(calibrator.getListener());
9213 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9214 assertFalse(calibrator.isReady());
9215 assertFalse(calibrator.isRunning());
9216 assertNull(calibrator.getEstimatedBiases());
9217 assertFalse(calibrator.getEstimatedBiases(null));
9218 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9219 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9220 assertNull(calibrator.getEstimatedBiasFx());
9221 assertNull(calibrator.getEstimatedBiasFy());
9222 assertNull(calibrator.getEstimatedBiasFz());
9223 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9224 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9225 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9226 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9227 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9228 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9229 assertNull(calibrator.getEstimatedMa());
9230 assertNull(calibrator.getEstimatedSx());
9231 assertNull(calibrator.getEstimatedSy());
9232 assertNull(calibrator.getEstimatedSz());
9233 assertNull(calibrator.getEstimatedMxy());
9234 assertNull(calibrator.getEstimatedMxz());
9235 assertNull(calibrator.getEstimatedMyx());
9236 assertNull(calibrator.getEstimatedMyz());
9237 assertNull(calibrator.getEstimatedMzx());
9238 assertNull(calibrator.getEstimatedMzy());
9239 assertNull(calibrator.getEstimatedCovariance());
9240 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9241 }
9242
9243 @Test
9244 public void testConstructor82() throws WrongSizeException {
9245 final Collection<StandardDeviationBodyKinematics> measurements =
9246 Collections.emptyList();
9247
9248 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9249 final double latitude = Math.toRadians(
9250 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9251 final double longitude = Math.toRadians(
9252 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9253 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9254 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9255 final NEDVelocity nedVelocity = new NEDVelocity();
9256 final ECEFPosition ecefPosition = new ECEFPosition();
9257 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9258 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9259 ecefPosition, ecefVelocity);
9260
9261 final KnownPositionAccelerometerCalibrator calibrator =
9262 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
9263 this);
9264
9265
9266 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9267 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9268 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9269 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9270 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9271 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9272 final Acceleration bx2 = new Acceleration(0.0,
9273 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9274 calibrator.getInitialBiasXAsAcceleration(bx2);
9275 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9276 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9277 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9278 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9279 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9280 final Acceleration by2 = new Acceleration(0.0,
9281 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9282 calibrator.getInitialBiasYAsAcceleration(by2);
9283 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9284 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9285 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9286 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9287 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9288 final Acceleration bz2 = new Acceleration(0.0,
9289 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9290 calibrator.getInitialBiasZAsAcceleration(bz2);
9291 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9292 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9293 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9294 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9295 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9296 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9297 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9298 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9299 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9300 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9301 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9302 final double[] bias1 = calibrator.getInitialBias();
9303 assertArrayEquals(bias1, new double[3], 0.0);
9304 final double[] bias2 = new double[3];
9305 calibrator.getInitialBias(bias2);
9306 assertArrayEquals(bias1, bias2, 0.0);
9307 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9308 assertEquals(b1, new Matrix(3, 1));
9309 final Matrix b2 = new Matrix(3, 1);
9310 calibrator.getInitialBiasAsMatrix(b2);
9311 assertEquals(b1, b2);
9312 final Matrix ma1 = calibrator.getInitialMa();
9313 assertEquals(ma1, new Matrix(3, 3));
9314 final Matrix ma2 = new Matrix(3, 3);
9315 calibrator.getInitialMa(ma2);
9316 assertEquals(ma1, ma2);
9317 assertSame(calibrator.getEcefPosition(), ecefPosition);
9318 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9319 final NEDPosition nedPosition1 = new NEDPosition();
9320 assertTrue(calibrator.getNedPosition(nedPosition1));
9321 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9322 assertSame(calibrator.getMeasurements(), measurements);
9323 assertFalse(calibrator.isCommonAxisUsed());
9324 assertSame(calibrator.getListener(), this);
9325 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9326 assertFalse(calibrator.isReady());
9327 assertFalse(calibrator.isRunning());
9328 assertNull(calibrator.getEstimatedBiases());
9329 assertFalse(calibrator.getEstimatedBiases(null));
9330 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9331 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9332 assertNull(calibrator.getEstimatedBiasFx());
9333 assertNull(calibrator.getEstimatedBiasFy());
9334 assertNull(calibrator.getEstimatedBiasFz());
9335 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9336 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9337 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9338 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9339 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9340 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9341 assertNull(calibrator.getEstimatedMa());
9342 assertNull(calibrator.getEstimatedSx());
9343 assertNull(calibrator.getEstimatedSy());
9344 assertNull(calibrator.getEstimatedSz());
9345 assertNull(calibrator.getEstimatedMxy());
9346 assertNull(calibrator.getEstimatedMxz());
9347 assertNull(calibrator.getEstimatedMyx());
9348 assertNull(calibrator.getEstimatedMyz());
9349 assertNull(calibrator.getEstimatedMzx());
9350 assertNull(calibrator.getEstimatedMzy());
9351 assertNull(calibrator.getEstimatedCovariance());
9352 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9353 }
9354
9355 @Test
9356 public void testConstructor83() throws WrongSizeException {
9357 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9358 final double latitude = Math.toRadians(
9359 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9360 final double longitude = Math.toRadians(
9361 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9362 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9363 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9364 final NEDVelocity nedVelocity = new NEDVelocity();
9365 final ECEFPosition ecefPosition = new ECEFPosition();
9366 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9367 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9368 ecefPosition, ecefVelocity);
9369
9370 final KnownPositionAccelerometerCalibrator calibrator =
9371 new KnownPositionAccelerometerCalibrator(ecefPosition,
9372 true);
9373
9374
9375 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9376 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9377 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9378 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9379 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9380 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9381 final Acceleration bx2 = new Acceleration(0.0,
9382 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9383 calibrator.getInitialBiasXAsAcceleration(bx2);
9384 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9385 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9386 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9387 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9388 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9389 final Acceleration by2 = new Acceleration(0.0,
9390 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9391 calibrator.getInitialBiasYAsAcceleration(by2);
9392 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9393 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9394 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9395 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9396 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9397 final Acceleration bz2 = new Acceleration(0.0,
9398 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9399 calibrator.getInitialBiasZAsAcceleration(bz2);
9400 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9401 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9402 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9403 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9404 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9405 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9406 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9407 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9408 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9409 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9410 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9411 final double[] bias1 = calibrator.getInitialBias();
9412 assertArrayEquals(bias1, new double[3], 0.0);
9413 final double[] bias2 = new double[3];
9414 calibrator.getInitialBias(bias2);
9415 assertArrayEquals(bias1, bias2, 0.0);
9416 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9417 assertEquals(b1, new Matrix(3, 1));
9418 final Matrix b2 = new Matrix(3, 1);
9419 calibrator.getInitialBiasAsMatrix(b2);
9420 assertEquals(b1, b2);
9421 final Matrix ma1 = calibrator.getInitialMa();
9422 assertEquals(ma1, new Matrix(3, 3));
9423 final Matrix ma2 = new Matrix(3, 3);
9424 calibrator.getInitialMa(ma2);
9425 assertEquals(ma1, ma2);
9426 assertSame(calibrator.getEcefPosition(), ecefPosition);
9427 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9428 final NEDPosition nedPosition1 = new NEDPosition();
9429 assertTrue(calibrator.getNedPosition(nedPosition1));
9430 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9431 assertNull(calibrator.getMeasurements());
9432 assertTrue(calibrator.isCommonAxisUsed());
9433 assertNull(calibrator.getListener());
9434 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9435 assertFalse(calibrator.isReady());
9436 assertFalse(calibrator.isRunning());
9437 assertNull(calibrator.getEstimatedBiases());
9438 assertFalse(calibrator.getEstimatedBiases(null));
9439 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9440 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9441 assertNull(calibrator.getEstimatedBiasFx());
9442 assertNull(calibrator.getEstimatedBiasFy());
9443 assertNull(calibrator.getEstimatedBiasFz());
9444 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9445 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9446 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9447 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9448 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9449 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9450 assertNull(calibrator.getEstimatedMa());
9451 assertNull(calibrator.getEstimatedSx());
9452 assertNull(calibrator.getEstimatedSy());
9453 assertNull(calibrator.getEstimatedSz());
9454 assertNull(calibrator.getEstimatedMxy());
9455 assertNull(calibrator.getEstimatedMxz());
9456 assertNull(calibrator.getEstimatedMyx());
9457 assertNull(calibrator.getEstimatedMyz());
9458 assertNull(calibrator.getEstimatedMzx());
9459 assertNull(calibrator.getEstimatedMzy());
9460 assertNull(calibrator.getEstimatedCovariance());
9461 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9462 }
9463
9464 @Test
9465 public void testConstructor84() throws WrongSizeException {
9466 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9467 final double latitude = Math.toRadians(
9468 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9469 final double longitude = Math.toRadians(
9470 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9471 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9472 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9473 final NEDVelocity nedVelocity = new NEDVelocity();
9474 final ECEFPosition ecefPosition = new ECEFPosition();
9475 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9476 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9477 ecefPosition, ecefVelocity);
9478
9479 final KnownPositionAccelerometerCalibrator calibrator =
9480 new KnownPositionAccelerometerCalibrator(ecefPosition,
9481 true, this);
9482
9483
9484 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9485 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9486 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9487 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9488 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9489 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9490 final Acceleration bx2 = new Acceleration(0.0,
9491 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9492 calibrator.getInitialBiasXAsAcceleration(bx2);
9493 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9494 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9495 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9496 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9497 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9498 final Acceleration by2 = new Acceleration(0.0,
9499 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9500 calibrator.getInitialBiasYAsAcceleration(by2);
9501 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9502 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9503 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9504 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9505 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9506 final Acceleration bz2 = new Acceleration(0.0,
9507 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9508 calibrator.getInitialBiasZAsAcceleration(bz2);
9509 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9510 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9511 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9512 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9513 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9514 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9515 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9516 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9517 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9518 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9519 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9520 final double[] bias1 = calibrator.getInitialBias();
9521 assertArrayEquals(bias1, new double[3], 0.0);
9522 final double[] bias2 = new double[3];
9523 calibrator.getInitialBias(bias2);
9524 assertArrayEquals(bias1, bias2, 0.0);
9525 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9526 assertEquals(b1, new Matrix(3, 1));
9527 final Matrix b2 = new Matrix(3, 1);
9528 calibrator.getInitialBiasAsMatrix(b2);
9529 assertEquals(b1, b2);
9530 final Matrix ma1 = calibrator.getInitialMa();
9531 assertEquals(ma1, new Matrix(3, 3));
9532 final Matrix ma2 = new Matrix(3, 3);
9533 calibrator.getInitialMa(ma2);
9534 assertEquals(ma1, ma2);
9535 assertSame(calibrator.getEcefPosition(), ecefPosition);
9536 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9537 final NEDPosition nedPosition1 = new NEDPosition();
9538 assertTrue(calibrator.getNedPosition(nedPosition1));
9539 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9540 assertNull(calibrator.getMeasurements());
9541 assertTrue(calibrator.isCommonAxisUsed());
9542 assertSame(calibrator.getListener(), this);
9543 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9544 assertFalse(calibrator.isReady());
9545 assertFalse(calibrator.isRunning());
9546 assertNull(calibrator.getEstimatedBiases());
9547 assertFalse(calibrator.getEstimatedBiases(null));
9548 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9549 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9550 assertNull(calibrator.getEstimatedBiasFx());
9551 assertNull(calibrator.getEstimatedBiasFy());
9552 assertNull(calibrator.getEstimatedBiasFz());
9553 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9554 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9555 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9556 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9557 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9558 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9559 assertNull(calibrator.getEstimatedMa());
9560 assertNull(calibrator.getEstimatedSx());
9561 assertNull(calibrator.getEstimatedSy());
9562 assertNull(calibrator.getEstimatedSz());
9563 assertNull(calibrator.getEstimatedMxy());
9564 assertNull(calibrator.getEstimatedMxz());
9565 assertNull(calibrator.getEstimatedMyx());
9566 assertNull(calibrator.getEstimatedMyz());
9567 assertNull(calibrator.getEstimatedMzx());
9568 assertNull(calibrator.getEstimatedMzy());
9569 assertNull(calibrator.getEstimatedCovariance());
9570 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9571 }
9572
9573 @Test
9574 public void testConstructor85() throws WrongSizeException {
9575 final Collection<StandardDeviationBodyKinematics> measurements =
9576 Collections.emptyList();
9577 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9578 final double latitude = Math.toRadians(
9579 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9580 final double longitude = Math.toRadians(
9581 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9582 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9583 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9584 final NEDVelocity nedVelocity = new NEDVelocity();
9585 final ECEFPosition ecefPosition = new ECEFPosition();
9586 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9587 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9588 ecefPosition, ecefVelocity);
9589
9590 final KnownPositionAccelerometerCalibrator calibrator =
9591 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
9592 true);
9593
9594
9595 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9596 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9597 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9598 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9599 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9600 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9601 final Acceleration bx2 = new Acceleration(0.0,
9602 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9603 calibrator.getInitialBiasXAsAcceleration(bx2);
9604 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9605 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9606 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9607 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9608 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9609 final Acceleration by2 = new Acceleration(0.0,
9610 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9611 calibrator.getInitialBiasYAsAcceleration(by2);
9612 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9613 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9614 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9615 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9616 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9617 final Acceleration bz2 = new Acceleration(0.0,
9618 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9619 calibrator.getInitialBiasZAsAcceleration(bz2);
9620 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9621 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9622 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9623 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9624 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9625 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9626 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9627 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9628 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9629 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9630 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9631 final double[] bias1 = calibrator.getInitialBias();
9632 assertArrayEquals(bias1, new double[3], 0.0);
9633 final double[] bias2 = new double[3];
9634 calibrator.getInitialBias(bias2);
9635 assertArrayEquals(bias1, bias2, 0.0);
9636 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9637 assertEquals(b1, new Matrix(3, 1));
9638 final Matrix b2 = new Matrix(3, 1);
9639 calibrator.getInitialBiasAsMatrix(b2);
9640 assertEquals(b1, b2);
9641 final Matrix ma1 = calibrator.getInitialMa();
9642 assertEquals(ma1, new Matrix(3, 3));
9643 final Matrix ma2 = new Matrix(3, 3);
9644 calibrator.getInitialMa(ma2);
9645 assertEquals(ma1, ma2);
9646 assertSame(calibrator.getEcefPosition(), ecefPosition);
9647 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9648 final NEDPosition nedPosition1 = new NEDPosition();
9649 assertTrue(calibrator.getNedPosition(nedPosition1));
9650 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9651 assertSame(calibrator.getMeasurements(), measurements);
9652 assertTrue(calibrator.isCommonAxisUsed());
9653 assertNull(calibrator.getListener());
9654 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9655 assertFalse(calibrator.isReady());
9656 assertFalse(calibrator.isRunning());
9657 assertNull(calibrator.getEstimatedBiases());
9658 assertFalse(calibrator.getEstimatedBiases(null));
9659 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9660 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9661 assertNull(calibrator.getEstimatedBiasFx());
9662 assertNull(calibrator.getEstimatedBiasFy());
9663 assertNull(calibrator.getEstimatedBiasFz());
9664 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9665 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9666 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9667 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9668 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9669 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9670 assertNull(calibrator.getEstimatedMa());
9671 assertNull(calibrator.getEstimatedSx());
9672 assertNull(calibrator.getEstimatedSy());
9673 assertNull(calibrator.getEstimatedSz());
9674 assertNull(calibrator.getEstimatedMxy());
9675 assertNull(calibrator.getEstimatedMxz());
9676 assertNull(calibrator.getEstimatedMyx());
9677 assertNull(calibrator.getEstimatedMyz());
9678 assertNull(calibrator.getEstimatedMzx());
9679 assertNull(calibrator.getEstimatedMzy());
9680 assertNull(calibrator.getEstimatedCovariance());
9681 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9682 }
9683
9684 @Test
9685 public void testConstructor86() throws WrongSizeException {
9686 final Collection<StandardDeviationBodyKinematics> measurements =
9687 Collections.emptyList();
9688 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9689 final double latitude = Math.toRadians(
9690 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9691 final double longitude = Math.toRadians(
9692 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9693 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9694 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9695 final NEDVelocity nedVelocity = new NEDVelocity();
9696 final ECEFPosition ecefPosition = new ECEFPosition();
9697 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9698 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9699 ecefPosition, ecefVelocity);
9700
9701 final KnownPositionAccelerometerCalibrator calibrator =
9702 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
9703 true, this);
9704
9705
9706 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9707 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9708 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9709 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9710 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9711 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9712 final Acceleration bx2 = new Acceleration(0.0,
9713 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9714 calibrator.getInitialBiasXAsAcceleration(bx2);
9715 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9716 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9717 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9718 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9719 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9720 final Acceleration by2 = new Acceleration(0.0,
9721 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9722 calibrator.getInitialBiasYAsAcceleration(by2);
9723 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9724 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9725 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9726 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9727 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9728 final Acceleration bz2 = new Acceleration(0.0,
9729 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9730 calibrator.getInitialBiasZAsAcceleration(bz2);
9731 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9732 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9733 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9734 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9735 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9736 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9737 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9738 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9739 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9740 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9741 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9742 final double[] bias1 = calibrator.getInitialBias();
9743 assertArrayEquals(bias1, new double[3], 0.0);
9744 final double[] bias2 = new double[3];
9745 calibrator.getInitialBias(bias2);
9746 assertArrayEquals(bias1, bias2, 0.0);
9747 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9748 assertEquals(b1, new Matrix(3, 1));
9749 final Matrix b2 = new Matrix(3, 1);
9750 calibrator.getInitialBiasAsMatrix(b2);
9751 assertEquals(b1, b2);
9752 final Matrix ma1 = calibrator.getInitialMa();
9753 assertEquals(ma1, new Matrix(3, 3));
9754 final Matrix ma2 = new Matrix(3, 3);
9755 calibrator.getInitialMa(ma2);
9756 assertEquals(ma1, ma2);
9757 assertSame(calibrator.getEcefPosition(), ecefPosition);
9758 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9759 final NEDPosition nedPosition1 = new NEDPosition();
9760 assertTrue(calibrator.getNedPosition(nedPosition1));
9761 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9762 assertSame(calibrator.getMeasurements(), measurements);
9763 assertTrue(calibrator.isCommonAxisUsed());
9764 assertSame(calibrator.getListener(), this);
9765 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9766 assertFalse(calibrator.isReady());
9767 assertFalse(calibrator.isRunning());
9768 assertNull(calibrator.getEstimatedBiases());
9769 assertFalse(calibrator.getEstimatedBiases(null));
9770 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9771 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9772 assertNull(calibrator.getEstimatedBiasFx());
9773 assertNull(calibrator.getEstimatedBiasFy());
9774 assertNull(calibrator.getEstimatedBiasFz());
9775 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9776 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9777 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9778 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9779 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9780 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9781 assertNull(calibrator.getEstimatedMa());
9782 assertNull(calibrator.getEstimatedSx());
9783 assertNull(calibrator.getEstimatedSy());
9784 assertNull(calibrator.getEstimatedSz());
9785 assertNull(calibrator.getEstimatedMxy());
9786 assertNull(calibrator.getEstimatedMxz());
9787 assertNull(calibrator.getEstimatedMyx());
9788 assertNull(calibrator.getEstimatedMyz());
9789 assertNull(calibrator.getEstimatedMzx());
9790 assertNull(calibrator.getEstimatedMzy());
9791 assertNull(calibrator.getEstimatedCovariance());
9792 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9793 }
9794
9795 @Test
9796 public void testConstructor87() throws WrongSizeException {
9797 final Matrix ba = generateBa();
9798 final double biasX = ba.getElementAtIndex(0);
9799 final double biasY = ba.getElementAtIndex(1);
9800 final double biasZ = ba.getElementAtIndex(2);
9801
9802 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9803 final double latitude = Math.toRadians(
9804 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9805 final double longitude = Math.toRadians(
9806 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9807 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9808 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9809 final NEDVelocity nedVelocity = new NEDVelocity();
9810 final ECEFPosition ecefPosition = new ECEFPosition();
9811 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9812 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9813 ecefPosition, ecefVelocity);
9814
9815 final KnownPositionAccelerometerCalibrator calibrator =
9816 new KnownPositionAccelerometerCalibrator(ecefPosition,
9817 biasX, biasY, biasZ);
9818
9819
9820 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
9821 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
9822 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
9823 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9824 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
9825 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9826 final Acceleration bx2 = new Acceleration(0.0,
9827 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9828 calibrator.getInitialBiasXAsAcceleration(bx2);
9829 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
9830 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9831 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9832 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
9833 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9834 final Acceleration by2 = new Acceleration(0.0,
9835 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9836 calibrator.getInitialBiasYAsAcceleration(by2);
9837 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
9838 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9839 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9840 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
9841 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9842 final Acceleration bz2 = new Acceleration(0.0,
9843 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9844 calibrator.getInitialBiasZAsAcceleration(bz2);
9845 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
9846 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9847 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9848 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9849 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9850 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9851 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9852 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9853 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9854 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9855 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9856 final double[] bias1 = calibrator.getInitialBias();
9857 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
9858 final double[] bias2 = new double[3];
9859 calibrator.getInitialBias(bias2);
9860 assertArrayEquals(bias1, bias2, 0.0);
9861 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9862 assertEquals(b1, ba);
9863 final Matrix b2 = new Matrix(3, 1);
9864 calibrator.getInitialBiasAsMatrix(b2);
9865 assertEquals(b1, b2);
9866 final Matrix ma1 = calibrator.getInitialMa();
9867 assertEquals(ma1, new Matrix(3, 3));
9868 final Matrix ma2 = new Matrix(3, 3);
9869 calibrator.getInitialMa(ma2);
9870 assertEquals(ma1, ma2);
9871 assertSame(calibrator.getEcefPosition(), ecefPosition);
9872 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9873 final NEDPosition nedPosition1 = new NEDPosition();
9874 assertTrue(calibrator.getNedPosition(nedPosition1));
9875 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9876 assertNull(calibrator.getMeasurements());
9877 assertFalse(calibrator.isCommonAxisUsed());
9878 assertNull(calibrator.getListener());
9879 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9880 assertFalse(calibrator.isReady());
9881 assertFalse(calibrator.isRunning());
9882 assertNull(calibrator.getEstimatedBiases());
9883 assertFalse(calibrator.getEstimatedBiases(null));
9884 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9885 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9886 assertNull(calibrator.getEstimatedBiasFx());
9887 assertNull(calibrator.getEstimatedBiasFy());
9888 assertNull(calibrator.getEstimatedBiasFz());
9889 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9890 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9891 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9892 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9893 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9894 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9895 assertNull(calibrator.getEstimatedMa());
9896 assertNull(calibrator.getEstimatedSx());
9897 assertNull(calibrator.getEstimatedSy());
9898 assertNull(calibrator.getEstimatedSz());
9899 assertNull(calibrator.getEstimatedMxy());
9900 assertNull(calibrator.getEstimatedMxz());
9901 assertNull(calibrator.getEstimatedMyx());
9902 assertNull(calibrator.getEstimatedMyz());
9903 assertNull(calibrator.getEstimatedMzx());
9904 assertNull(calibrator.getEstimatedMzy());
9905 assertNull(calibrator.getEstimatedCovariance());
9906 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9907 }
9908
9909 @Test
9910 public void testConstructor88() throws WrongSizeException {
9911 final Matrix ba = generateBa();
9912 final double biasX = ba.getElementAtIndex(0);
9913 final double biasY = ba.getElementAtIndex(1);
9914 final double biasZ = ba.getElementAtIndex(2);
9915
9916 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9917 final double latitude = Math.toRadians(
9918 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9919 final double longitude = Math.toRadians(
9920 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9921 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9922 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9923 final NEDVelocity nedVelocity = new NEDVelocity();
9924 final ECEFPosition ecefPosition = new ECEFPosition();
9925 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9926 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9927 ecefPosition, ecefVelocity);
9928
9929 final KnownPositionAccelerometerCalibrator calibrator =
9930 new KnownPositionAccelerometerCalibrator(ecefPosition,
9931 biasX, biasY, biasZ, this);
9932
9933
9934 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
9935 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
9936 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
9937 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9938 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
9939 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9940 final Acceleration bx2 = new Acceleration(0.0,
9941 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9942 calibrator.getInitialBiasXAsAcceleration(bx2);
9943 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
9944 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9945 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9946 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
9947 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9948 final Acceleration by2 = new Acceleration(0.0,
9949 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9950 calibrator.getInitialBiasYAsAcceleration(by2);
9951 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
9952 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9953 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9954 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
9955 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9956 final Acceleration bz2 = new Acceleration(0.0,
9957 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9958 calibrator.getInitialBiasZAsAcceleration(bz2);
9959 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
9960 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9961 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9962 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9963 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9964 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9965 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9966 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9967 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9968 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9969 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9970 final double[] bias1 = calibrator.getInitialBias();
9971 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
9972 final double[] bias2 = new double[3];
9973 calibrator.getInitialBias(bias2);
9974 assertArrayEquals(bias1, bias2, 0.0);
9975 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9976 assertEquals(b1, ba);
9977 final Matrix b2 = new Matrix(3, 1);
9978 calibrator.getInitialBiasAsMatrix(b2);
9979 assertEquals(b1, b2);
9980 final Matrix ma1 = calibrator.getInitialMa();
9981 assertEquals(ma1, new Matrix(3, 3));
9982 final Matrix ma2 = new Matrix(3, 3);
9983 calibrator.getInitialMa(ma2);
9984 assertEquals(ma1, ma2);
9985 assertSame(calibrator.getEcefPosition(), ecefPosition);
9986 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9987 final NEDPosition nedPosition1 = new NEDPosition();
9988 assertTrue(calibrator.getNedPosition(nedPosition1));
9989 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9990 assertNull(calibrator.getMeasurements());
9991 assertFalse(calibrator.isCommonAxisUsed());
9992 assertSame(calibrator.getListener(), this);
9993 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9994 assertFalse(calibrator.isReady());
9995 assertFalse(calibrator.isRunning());
9996 assertNull(calibrator.getEstimatedBiases());
9997 assertFalse(calibrator.getEstimatedBiases(null));
9998 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9999 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10000 assertNull(calibrator.getEstimatedBiasFx());
10001 assertNull(calibrator.getEstimatedBiasFy());
10002 assertNull(calibrator.getEstimatedBiasFz());
10003 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10004 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10005 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10006 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10007 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10008 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10009 assertNull(calibrator.getEstimatedMa());
10010 assertNull(calibrator.getEstimatedSx());
10011 assertNull(calibrator.getEstimatedSy());
10012 assertNull(calibrator.getEstimatedSz());
10013 assertNull(calibrator.getEstimatedMxy());
10014 assertNull(calibrator.getEstimatedMxz());
10015 assertNull(calibrator.getEstimatedMyx());
10016 assertNull(calibrator.getEstimatedMyz());
10017 assertNull(calibrator.getEstimatedMzx());
10018 assertNull(calibrator.getEstimatedMzy());
10019 assertNull(calibrator.getEstimatedCovariance());
10020 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10021 }
10022
10023 @Test
10024 public void testConstructor89() throws WrongSizeException {
10025 final Collection<StandardDeviationBodyKinematics> measurements =
10026 Collections.emptyList();
10027
10028 final Matrix ba = generateBa();
10029 final double biasX = ba.getElementAtIndex(0);
10030 final double biasY = ba.getElementAtIndex(1);
10031 final double biasZ = ba.getElementAtIndex(2);
10032
10033 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10034 final double latitude = Math.toRadians(
10035 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10036 final double longitude = Math.toRadians(
10037 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10038 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10039 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10040 final NEDVelocity nedVelocity = new NEDVelocity();
10041 final ECEFPosition ecefPosition = new ECEFPosition();
10042 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10043 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10044 ecefPosition, ecefVelocity);
10045
10046 final KnownPositionAccelerometerCalibrator calibrator =
10047 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
10048 biasX, biasY, biasZ);
10049
10050
10051 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10052 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10053 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10054 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10055 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10056 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10057 final Acceleration bx2 = new Acceleration(0.0,
10058 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10059 calibrator.getInitialBiasXAsAcceleration(bx2);
10060 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10061 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10062 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10063 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10064 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10065 final Acceleration by2 = new Acceleration(0.0,
10066 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10067 calibrator.getInitialBiasYAsAcceleration(by2);
10068 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10069 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10070 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10071 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10072 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10073 final Acceleration bz2 = new Acceleration(0.0,
10074 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10075 calibrator.getInitialBiasZAsAcceleration(bz2);
10076 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10077 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10078 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10079 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10080 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10081 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10082 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10083 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10084 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10085 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10086 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10087 final double[] bias1 = calibrator.getInitialBias();
10088 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
10089 final double[] bias2 = new double[3];
10090 calibrator.getInitialBias(bias2);
10091 assertArrayEquals(bias1, bias2, 0.0);
10092 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10093 assertEquals(b1, ba);
10094 final Matrix b2 = new Matrix(3, 1);
10095 calibrator.getInitialBiasAsMatrix(b2);
10096 assertEquals(b1, b2);
10097 final Matrix ma1 = calibrator.getInitialMa();
10098 assertEquals(ma1, new Matrix(3, 3));
10099 final Matrix ma2 = new Matrix(3, 3);
10100 calibrator.getInitialMa(ma2);
10101 assertEquals(ma1, ma2);
10102 assertSame(calibrator.getEcefPosition(), ecefPosition);
10103 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10104 final NEDPosition nedPosition1 = new NEDPosition();
10105 assertTrue(calibrator.getNedPosition(nedPosition1));
10106 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10107 assertSame(calibrator.getMeasurements(), measurements);
10108 assertFalse(calibrator.isCommonAxisUsed());
10109 assertNull(calibrator.getListener());
10110 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10111 assertFalse(calibrator.isReady());
10112 assertFalse(calibrator.isRunning());
10113 assertNull(calibrator.getEstimatedBiases());
10114 assertFalse(calibrator.getEstimatedBiases(null));
10115 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10116 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10117 assertNull(calibrator.getEstimatedBiasFx());
10118 assertNull(calibrator.getEstimatedBiasFy());
10119 assertNull(calibrator.getEstimatedBiasFz());
10120 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10121 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10122 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10123 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10124 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10125 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10126 assertNull(calibrator.getEstimatedMa());
10127 assertNull(calibrator.getEstimatedSx());
10128 assertNull(calibrator.getEstimatedSy());
10129 assertNull(calibrator.getEstimatedSz());
10130 assertNull(calibrator.getEstimatedMxy());
10131 assertNull(calibrator.getEstimatedMxz());
10132 assertNull(calibrator.getEstimatedMyx());
10133 assertNull(calibrator.getEstimatedMyz());
10134 assertNull(calibrator.getEstimatedMzx());
10135 assertNull(calibrator.getEstimatedMzy());
10136 assertNull(calibrator.getEstimatedCovariance());
10137 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10138 }
10139
10140 @Test
10141 public void testConstructor90() throws WrongSizeException {
10142 final Collection<StandardDeviationBodyKinematics> measurements =
10143 Collections.emptyList();
10144
10145 final Matrix ba = generateBa();
10146 final double biasX = ba.getElementAtIndex(0);
10147 final double biasY = ba.getElementAtIndex(1);
10148 final double biasZ = ba.getElementAtIndex(2);
10149
10150 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10151 final double latitude = Math.toRadians(
10152 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10153 final double longitude = Math.toRadians(
10154 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10155 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10156 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10157 final NEDVelocity nedVelocity = new NEDVelocity();
10158 final ECEFPosition ecefPosition = new ECEFPosition();
10159 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10160 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10161 ecefPosition, ecefVelocity);
10162
10163 final KnownPositionAccelerometerCalibrator calibrator =
10164 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
10165 biasX, biasY, biasZ, this);
10166
10167
10168 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10169 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10170 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10171 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10172 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10173 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10174 final Acceleration bx2 = new Acceleration(0.0,
10175 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10176 calibrator.getInitialBiasXAsAcceleration(bx2);
10177 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10178 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10179 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10180 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10181 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10182 final Acceleration by2 = new Acceleration(0.0,
10183 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10184 calibrator.getInitialBiasYAsAcceleration(by2);
10185 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10186 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10187 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10188 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10189 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10190 final Acceleration bz2 = new Acceleration(0.0,
10191 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10192 calibrator.getInitialBiasZAsAcceleration(bz2);
10193 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10194 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10195 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10196 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10197 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10198 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10199 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10200 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10201 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10202 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10203 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10204 final double[] bias1 = calibrator.getInitialBias();
10205 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
10206 final double[] bias2 = new double[3];
10207 calibrator.getInitialBias(bias2);
10208 assertArrayEquals(bias1, bias2, 0.0);
10209 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10210 assertEquals(b1, ba);
10211 final Matrix b2 = new Matrix(3, 1);
10212 calibrator.getInitialBiasAsMatrix(b2);
10213 assertEquals(b1, b2);
10214 final Matrix ma1 = calibrator.getInitialMa();
10215 assertEquals(ma1, new Matrix(3, 3));
10216 final Matrix ma2 = new Matrix(3, 3);
10217 calibrator.getInitialMa(ma2);
10218 assertEquals(ma1, ma2);
10219 assertSame(calibrator.getEcefPosition(), ecefPosition);
10220 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10221 final NEDPosition nedPosition1 = new NEDPosition();
10222 assertTrue(calibrator.getNedPosition(nedPosition1));
10223 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10224 assertSame(calibrator.getMeasurements(), measurements);
10225 assertFalse(calibrator.isCommonAxisUsed());
10226 assertSame(calibrator.getListener(), this);
10227 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10228 assertFalse(calibrator.isReady());
10229 assertFalse(calibrator.isRunning());
10230 assertNull(calibrator.getEstimatedBiases());
10231 assertFalse(calibrator.getEstimatedBiases(null));
10232 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10233 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10234 assertNull(calibrator.getEstimatedBiasFx());
10235 assertNull(calibrator.getEstimatedBiasFy());
10236 assertNull(calibrator.getEstimatedBiasFz());
10237 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10238 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10239 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10240 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10241 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10242 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10243 assertNull(calibrator.getEstimatedMa());
10244 assertNull(calibrator.getEstimatedSx());
10245 assertNull(calibrator.getEstimatedSy());
10246 assertNull(calibrator.getEstimatedSz());
10247 assertNull(calibrator.getEstimatedMxy());
10248 assertNull(calibrator.getEstimatedMxz());
10249 assertNull(calibrator.getEstimatedMyx());
10250 assertNull(calibrator.getEstimatedMyz());
10251 assertNull(calibrator.getEstimatedMzx());
10252 assertNull(calibrator.getEstimatedMzy());
10253 assertNull(calibrator.getEstimatedCovariance());
10254 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10255 }
10256
10257 @Test
10258 public void testConstructor91() throws WrongSizeException {
10259 final Matrix ba = generateBa();
10260 final double biasX = ba.getElementAtIndex(0);
10261 final double biasY = ba.getElementAtIndex(1);
10262 final double biasZ = ba.getElementAtIndex(2);
10263
10264 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10265 final double latitude = Math.toRadians(
10266 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10267 final double longitude = Math.toRadians(
10268 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10269 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10270 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10271 final NEDVelocity nedVelocity = new NEDVelocity();
10272 final ECEFPosition ecefPosition = new ECEFPosition();
10273 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10274 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10275 ecefPosition, ecefVelocity);
10276
10277 final KnownPositionAccelerometerCalibrator calibrator =
10278 new KnownPositionAccelerometerCalibrator(ecefPosition,
10279 true, biasX, biasY, biasZ);
10280
10281
10282 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10283 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10284 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10285 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10286 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10287 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10288 final Acceleration bx2 = new Acceleration(0.0,
10289 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10290 calibrator.getInitialBiasXAsAcceleration(bx2);
10291 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10292 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10293 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10294 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10295 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10296 final Acceleration by2 = new Acceleration(0.0,
10297 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10298 calibrator.getInitialBiasYAsAcceleration(by2);
10299 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10300 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10301 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10302 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10303 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10304 final Acceleration bz2 = new Acceleration(0.0,
10305 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10306 calibrator.getInitialBiasZAsAcceleration(bz2);
10307 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10308 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10309 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10310 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10311 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10312 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10313 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10314 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10315 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10316 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10317 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10318 final double[] bias1 = calibrator.getInitialBias();
10319 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
10320 final double[] bias2 = new double[3];
10321 calibrator.getInitialBias(bias2);
10322 assertArrayEquals(bias1, bias2, 0.0);
10323 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10324 assertEquals(b1, ba);
10325 final Matrix b2 = new Matrix(3, 1);
10326 calibrator.getInitialBiasAsMatrix(b2);
10327 assertEquals(b1, b2);
10328 final Matrix ma1 = calibrator.getInitialMa();
10329 assertEquals(ma1, new Matrix(3, 3));
10330 final Matrix ma2 = new Matrix(3, 3);
10331 calibrator.getInitialMa(ma2);
10332 assertEquals(ma1, ma2);
10333 assertSame(calibrator.getEcefPosition(), ecefPosition);
10334 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10335 final NEDPosition nedPosition1 = new NEDPosition();
10336 assertTrue(calibrator.getNedPosition(nedPosition1));
10337 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10338 assertNull(calibrator.getMeasurements());
10339 assertTrue(calibrator.isCommonAxisUsed());
10340 assertNull(calibrator.getListener());
10341 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10342 assertFalse(calibrator.isReady());
10343 assertFalse(calibrator.isRunning());
10344 assertNull(calibrator.getEstimatedBiases());
10345 assertFalse(calibrator.getEstimatedBiases(null));
10346 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10347 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10348 assertNull(calibrator.getEstimatedBiasFx());
10349 assertNull(calibrator.getEstimatedBiasFy());
10350 assertNull(calibrator.getEstimatedBiasFz());
10351 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10352 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10353 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10354 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10355 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10356 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10357 assertNull(calibrator.getEstimatedMa());
10358 assertNull(calibrator.getEstimatedSx());
10359 assertNull(calibrator.getEstimatedSy());
10360 assertNull(calibrator.getEstimatedSz());
10361 assertNull(calibrator.getEstimatedMxy());
10362 assertNull(calibrator.getEstimatedMxz());
10363 assertNull(calibrator.getEstimatedMyx());
10364 assertNull(calibrator.getEstimatedMyz());
10365 assertNull(calibrator.getEstimatedMzx());
10366 assertNull(calibrator.getEstimatedMzy());
10367 assertNull(calibrator.getEstimatedCovariance());
10368 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10369 }
10370
10371 @Test
10372 public void testConstructor92() throws WrongSizeException {
10373 final Matrix ba = generateBa();
10374 final double biasX = ba.getElementAtIndex(0);
10375 final double biasY = ba.getElementAtIndex(1);
10376 final double biasZ = ba.getElementAtIndex(2);
10377
10378 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10379 final double latitude = Math.toRadians(
10380 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10381 final double longitude = Math.toRadians(
10382 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10383 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10384 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10385 final NEDVelocity nedVelocity = new NEDVelocity();
10386 final ECEFPosition ecefPosition = new ECEFPosition();
10387 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10388 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10389 ecefPosition, ecefVelocity);
10390
10391 final KnownPositionAccelerometerCalibrator calibrator =
10392 new KnownPositionAccelerometerCalibrator(ecefPosition,
10393 true, biasX, biasY, biasZ, this);
10394
10395
10396 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10397 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10398 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10399 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10400 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10401 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10402 final Acceleration bx2 = new Acceleration(0.0,
10403 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10404 calibrator.getInitialBiasXAsAcceleration(bx2);
10405 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10406 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10407 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10408 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10409 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10410 final Acceleration by2 = new Acceleration(0.0,
10411 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10412 calibrator.getInitialBiasYAsAcceleration(by2);
10413 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10414 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10415 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10416 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10417 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10418 final Acceleration bz2 = new Acceleration(0.0,
10419 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10420 calibrator.getInitialBiasZAsAcceleration(bz2);
10421 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10422 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10423 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10424 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10425 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10426 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10427 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10428 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10429 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10430 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10431 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10432 final double[] bias1 = calibrator.getInitialBias();
10433 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
10434 final double[] bias2 = new double[3];
10435 calibrator.getInitialBias(bias2);
10436 assertArrayEquals(bias1, bias2, 0.0);
10437 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10438 assertEquals(b1, ba);
10439 final Matrix b2 = new Matrix(3, 1);
10440 calibrator.getInitialBiasAsMatrix(b2);
10441 assertEquals(b1, b2);
10442 final Matrix ma1 = calibrator.getInitialMa();
10443 assertEquals(ma1, new Matrix(3, 3));
10444 final Matrix ma2 = new Matrix(3, 3);
10445 calibrator.getInitialMa(ma2);
10446 assertEquals(ma1, ma2);
10447 assertSame(calibrator.getEcefPosition(), ecefPosition);
10448 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10449 final NEDPosition nedPosition1 = new NEDPosition();
10450 assertTrue(calibrator.getNedPosition(nedPosition1));
10451 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10452 assertNull(calibrator.getMeasurements());
10453 assertTrue(calibrator.isCommonAxisUsed());
10454 assertSame(calibrator.getListener(), this);
10455 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10456 assertFalse(calibrator.isReady());
10457 assertFalse(calibrator.isRunning());
10458 assertNull(calibrator.getEstimatedBiases());
10459 assertFalse(calibrator.getEstimatedBiases(null));
10460 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10461 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10462 assertNull(calibrator.getEstimatedBiasFx());
10463 assertNull(calibrator.getEstimatedBiasFy());
10464 assertNull(calibrator.getEstimatedBiasFz());
10465 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10466 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10467 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10468 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10469 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10470 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10471 assertNull(calibrator.getEstimatedMa());
10472 assertNull(calibrator.getEstimatedSx());
10473 assertNull(calibrator.getEstimatedSy());
10474 assertNull(calibrator.getEstimatedSz());
10475 assertNull(calibrator.getEstimatedMxy());
10476 assertNull(calibrator.getEstimatedMxz());
10477 assertNull(calibrator.getEstimatedMyx());
10478 assertNull(calibrator.getEstimatedMyz());
10479 assertNull(calibrator.getEstimatedMzx());
10480 assertNull(calibrator.getEstimatedMzy());
10481 assertNull(calibrator.getEstimatedCovariance());
10482 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10483 }
10484
10485 @Test
10486 public void testConstructor93() throws WrongSizeException {
10487 final Collection<StandardDeviationBodyKinematics> measurements =
10488 Collections.emptyList();
10489
10490 final Matrix ba = generateBa();
10491 final double biasX = ba.getElementAtIndex(0);
10492 final double biasY = ba.getElementAtIndex(1);
10493 final double biasZ = ba.getElementAtIndex(2);
10494
10495 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10496 final double latitude = Math.toRadians(
10497 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10498 final double longitude = Math.toRadians(
10499 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10500 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10501 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10502 final NEDVelocity nedVelocity = new NEDVelocity();
10503 final ECEFPosition ecefPosition = new ECEFPosition();
10504 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10505 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10506 ecefPosition, ecefVelocity);
10507
10508 final KnownPositionAccelerometerCalibrator calibrator =
10509 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
10510 true, biasX, biasY, biasZ);
10511
10512
10513 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10514 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10515 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10516 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10517 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10518 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10519 final Acceleration bx2 = new Acceleration(0.0,
10520 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10521 calibrator.getInitialBiasXAsAcceleration(bx2);
10522 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10523 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10524 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10525 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10526 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10527 final Acceleration by2 = new Acceleration(0.0,
10528 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10529 calibrator.getInitialBiasYAsAcceleration(by2);
10530 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10531 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10532 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10533 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10534 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10535 final Acceleration bz2 = new Acceleration(0.0,
10536 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10537 calibrator.getInitialBiasZAsAcceleration(bz2);
10538 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10539 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10540 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10541 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10542 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10543 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10544 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10545 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10546 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10547 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10548 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10549 final double[] bias1 = calibrator.getInitialBias();
10550 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
10551 final double[] bias2 = new double[3];
10552 calibrator.getInitialBias(bias2);
10553 assertArrayEquals(bias1, bias2, 0.0);
10554 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10555 assertEquals(b1, ba);
10556 final Matrix b2 = new Matrix(3, 1);
10557 calibrator.getInitialBiasAsMatrix(b2);
10558 assertEquals(b1, b2);
10559 final Matrix ma1 = calibrator.getInitialMa();
10560 assertEquals(ma1, new Matrix(3, 3));
10561 final Matrix ma2 = new Matrix(3, 3);
10562 calibrator.getInitialMa(ma2);
10563 assertEquals(ma1, ma2);
10564 assertSame(calibrator.getEcefPosition(), ecefPosition);
10565 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10566 final NEDPosition nedPosition1 = new NEDPosition();
10567 assertTrue(calibrator.getNedPosition(nedPosition1));
10568 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10569 assertSame(calibrator.getMeasurements(), measurements);
10570 assertTrue(calibrator.isCommonAxisUsed());
10571 assertNull(calibrator.getListener());
10572 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10573 assertFalse(calibrator.isReady());
10574 assertFalse(calibrator.isRunning());
10575 assertNull(calibrator.getEstimatedBiases());
10576 assertFalse(calibrator.getEstimatedBiases(null));
10577 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10578 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10579 assertNull(calibrator.getEstimatedBiasFx());
10580 assertNull(calibrator.getEstimatedBiasFy());
10581 assertNull(calibrator.getEstimatedBiasFz());
10582 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10583 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10584 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10585 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10586 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10587 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10588 assertNull(calibrator.getEstimatedMa());
10589 assertNull(calibrator.getEstimatedSx());
10590 assertNull(calibrator.getEstimatedSy());
10591 assertNull(calibrator.getEstimatedSz());
10592 assertNull(calibrator.getEstimatedMxy());
10593 assertNull(calibrator.getEstimatedMxz());
10594 assertNull(calibrator.getEstimatedMyx());
10595 assertNull(calibrator.getEstimatedMyz());
10596 assertNull(calibrator.getEstimatedMzx());
10597 assertNull(calibrator.getEstimatedMzy());
10598 assertNull(calibrator.getEstimatedCovariance());
10599 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10600 }
10601
10602 @Test
10603 public void testConstructor94() throws WrongSizeException {
10604 final Collection<StandardDeviationBodyKinematics> measurements =
10605 Collections.emptyList();
10606
10607 final Matrix ba = generateBa();
10608 final double biasX = ba.getElementAtIndex(0);
10609 final double biasY = ba.getElementAtIndex(1);
10610 final double biasZ = ba.getElementAtIndex(2);
10611
10612 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10613 final double latitude = Math.toRadians(
10614 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10615 final double longitude = Math.toRadians(
10616 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10617 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10618 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10619 final NEDVelocity nedVelocity = new NEDVelocity();
10620 final ECEFPosition ecefPosition = new ECEFPosition();
10621 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10622 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10623 ecefPosition, ecefVelocity);
10624
10625 final KnownPositionAccelerometerCalibrator calibrator =
10626 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
10627 true, biasX, biasY, biasZ, this);
10628
10629
10630 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10631 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10632 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10633 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10634 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10635 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10636 final Acceleration bx2 = new Acceleration(0.0,
10637 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10638 calibrator.getInitialBiasXAsAcceleration(bx2);
10639 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10640 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10641 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10642 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10643 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10644 final Acceleration by2 = new Acceleration(0.0,
10645 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10646 calibrator.getInitialBiasYAsAcceleration(by2);
10647 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10648 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10649 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10650 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10651 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10652 final Acceleration bz2 = new Acceleration(0.0,
10653 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10654 calibrator.getInitialBiasZAsAcceleration(bz2);
10655 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10656 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10657 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10658 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10659 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10660 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10661 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10662 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10663 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10664 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10665 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10666 final double[] bias1 = calibrator.getInitialBias();
10667 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
10668 final double[] bias2 = new double[3];
10669 calibrator.getInitialBias(bias2);
10670 assertArrayEquals(bias1, bias2, 0.0);
10671 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10672 assertEquals(b1, ba);
10673 final Matrix b2 = new Matrix(3, 1);
10674 calibrator.getInitialBiasAsMatrix(b2);
10675 assertEquals(b1, b2);
10676 final Matrix ma1 = calibrator.getInitialMa();
10677 assertEquals(ma1, new Matrix(3, 3));
10678 final Matrix ma2 = new Matrix(3, 3);
10679 calibrator.getInitialMa(ma2);
10680 assertEquals(ma1, ma2);
10681 assertSame(calibrator.getEcefPosition(), ecefPosition);
10682 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10683 final NEDPosition nedPosition1 = new NEDPosition();
10684 assertTrue(calibrator.getNedPosition(nedPosition1));
10685 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10686 assertSame(calibrator.getMeasurements(), measurements);
10687 assertTrue(calibrator.isCommonAxisUsed());
10688 assertSame(calibrator.getListener(), this);
10689 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10690 assertFalse(calibrator.isReady());
10691 assertFalse(calibrator.isRunning());
10692 assertNull(calibrator.getEstimatedBiases());
10693 assertFalse(calibrator.getEstimatedBiases(null));
10694 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10695 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10696 assertNull(calibrator.getEstimatedBiasFx());
10697 assertNull(calibrator.getEstimatedBiasFy());
10698 assertNull(calibrator.getEstimatedBiasFz());
10699 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10700 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10701 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10702 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10703 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10704 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10705 assertNull(calibrator.getEstimatedMa());
10706 assertNull(calibrator.getEstimatedSx());
10707 assertNull(calibrator.getEstimatedSy());
10708 assertNull(calibrator.getEstimatedSz());
10709 assertNull(calibrator.getEstimatedMxy());
10710 assertNull(calibrator.getEstimatedMxz());
10711 assertNull(calibrator.getEstimatedMyx());
10712 assertNull(calibrator.getEstimatedMyz());
10713 assertNull(calibrator.getEstimatedMzx());
10714 assertNull(calibrator.getEstimatedMzy());
10715 assertNull(calibrator.getEstimatedCovariance());
10716 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10717 }
10718
10719 @Test
10720 public void testConstructor95() throws WrongSizeException {
10721 final Matrix ba = generateBa();
10722 final double biasX = ba.getElementAtIndex(0);
10723 final double biasY = ba.getElementAtIndex(1);
10724 final double biasZ = ba.getElementAtIndex(2);
10725
10726 final Acceleration bx = new Acceleration(biasX,
10727 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10728 final Acceleration by = new Acceleration(biasY,
10729 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10730 final Acceleration bz = new Acceleration(biasZ,
10731 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10732
10733 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10734 final double latitude = Math.toRadians(
10735 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10736 final double longitude = Math.toRadians(
10737 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10738 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10739 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10740 final NEDVelocity nedVelocity = new NEDVelocity();
10741 final ECEFPosition ecefPosition = new ECEFPosition();
10742 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10743 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10744 ecefPosition, ecefVelocity);
10745
10746 final KnownPositionAccelerometerCalibrator calibrator =
10747 new KnownPositionAccelerometerCalibrator(ecefPosition,
10748 bx, by, bz);
10749
10750
10751 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10752 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10753 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10754 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10755 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10756 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10757 final Acceleration bx2 = new Acceleration(0.0,
10758 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10759 calibrator.getInitialBiasXAsAcceleration(bx2);
10760 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10761 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10762 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10763 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10764 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10765 final Acceleration by2 = new Acceleration(0.0,
10766 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10767 calibrator.getInitialBiasYAsAcceleration(by2);
10768 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10769 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10770 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10771 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10772 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10773 final Acceleration bz2 = new Acceleration(0.0,
10774 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10775 calibrator.getInitialBiasZAsAcceleration(bz2);
10776 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10777 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10778 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10779 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10780 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10781 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10782 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10783 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10784 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10785 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10786 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10787 final double[] bias1 = calibrator.getInitialBias();
10788 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
10789 final double[] bias2 = new double[3];
10790 calibrator.getInitialBias(bias2);
10791 assertArrayEquals(bias1, bias2, 0.0);
10792 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10793 assertEquals(b1, ba);
10794 final Matrix b2 = new Matrix(3, 1);
10795 calibrator.getInitialBiasAsMatrix(b2);
10796 assertEquals(b1, b2);
10797 final Matrix ma1 = calibrator.getInitialMa();
10798 assertEquals(ma1, new Matrix(3, 3));
10799 final Matrix ma2 = new Matrix(3, 3);
10800 calibrator.getInitialMa(ma2);
10801 assertEquals(ma1, ma2);
10802 assertSame(calibrator.getEcefPosition(), ecefPosition);
10803 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10804 final NEDPosition nedPosition1 = new NEDPosition();
10805 assertTrue(calibrator.getNedPosition(nedPosition1));
10806 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10807 assertNull(calibrator.getMeasurements());
10808 assertFalse(calibrator.isCommonAxisUsed());
10809 assertNull(calibrator.getListener());
10810 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10811 assertFalse(calibrator.isReady());
10812 assertFalse(calibrator.isRunning());
10813 assertNull(calibrator.getEstimatedBiases());
10814 assertFalse(calibrator.getEstimatedBiases(null));
10815 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10816 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10817 assertNull(calibrator.getEstimatedBiasFx());
10818 assertNull(calibrator.getEstimatedBiasFy());
10819 assertNull(calibrator.getEstimatedBiasFz());
10820 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10821 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10822 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10823 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10824 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10825 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10826 assertNull(calibrator.getEstimatedMa());
10827 assertNull(calibrator.getEstimatedSx());
10828 assertNull(calibrator.getEstimatedSy());
10829 assertNull(calibrator.getEstimatedSz());
10830 assertNull(calibrator.getEstimatedMxy());
10831 assertNull(calibrator.getEstimatedMxz());
10832 assertNull(calibrator.getEstimatedMyx());
10833 assertNull(calibrator.getEstimatedMyz());
10834 assertNull(calibrator.getEstimatedMzx());
10835 assertNull(calibrator.getEstimatedMzy());
10836 assertNull(calibrator.getEstimatedCovariance());
10837 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10838 }
10839
10840 @Test
10841 public void testConstructor96() throws WrongSizeException {
10842 final Matrix ba = generateBa();
10843 final double biasX = ba.getElementAtIndex(0);
10844 final double biasY = ba.getElementAtIndex(1);
10845 final double biasZ = ba.getElementAtIndex(2);
10846
10847 final Acceleration bx = new Acceleration(biasX,
10848 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10849 final Acceleration by = new Acceleration(biasY,
10850 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10851 final Acceleration bz = new Acceleration(biasZ,
10852 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10853
10854 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10855 final double latitude = Math.toRadians(
10856 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10857 final double longitude = Math.toRadians(
10858 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10859 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10860 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10861 final NEDVelocity nedVelocity = new NEDVelocity();
10862 final ECEFPosition ecefPosition = new ECEFPosition();
10863 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10864 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10865 ecefPosition, ecefVelocity);
10866
10867 final KnownPositionAccelerometerCalibrator calibrator =
10868 new KnownPositionAccelerometerCalibrator(ecefPosition,
10869 bx, by, bz, this);
10870
10871
10872 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10873 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10874 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10875 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10876 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10877 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10878 final Acceleration bx2 = new Acceleration(0.0,
10879 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10880 calibrator.getInitialBiasXAsAcceleration(bx2);
10881 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10882 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10883 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10884 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10885 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10886 final Acceleration by2 = new Acceleration(0.0,
10887 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10888 calibrator.getInitialBiasYAsAcceleration(by2);
10889 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10890 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10891 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10892 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10893 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10894 final Acceleration bz2 = new Acceleration(0.0,
10895 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10896 calibrator.getInitialBiasZAsAcceleration(bz2);
10897 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10898 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10899 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10900 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10901 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10902 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10903 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10904 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10905 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10906 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10907 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10908 final double[] bias1 = calibrator.getInitialBias();
10909 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
10910 final double[] bias2 = new double[3];
10911 calibrator.getInitialBias(bias2);
10912 assertArrayEquals(bias1, bias2, 0.0);
10913 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10914 assertEquals(b1, ba);
10915 final Matrix b2 = new Matrix(3, 1);
10916 calibrator.getInitialBiasAsMatrix(b2);
10917 assertEquals(b1, b2);
10918 final Matrix ma1 = calibrator.getInitialMa();
10919 assertEquals(ma1, new Matrix(3, 3));
10920 final Matrix ma2 = new Matrix(3, 3);
10921 calibrator.getInitialMa(ma2);
10922 assertEquals(ma1, ma2);
10923 assertSame(calibrator.getEcefPosition(), ecefPosition);
10924 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10925 final NEDPosition nedPosition1 = new NEDPosition();
10926 assertTrue(calibrator.getNedPosition(nedPosition1));
10927 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10928 assertNull(calibrator.getMeasurements());
10929 assertFalse(calibrator.isCommonAxisUsed());
10930 assertSame(calibrator.getListener(), this);
10931 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10932 assertFalse(calibrator.isReady());
10933 assertFalse(calibrator.isRunning());
10934 assertNull(calibrator.getEstimatedBiases());
10935 assertFalse(calibrator.getEstimatedBiases(null));
10936 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10937 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10938 assertNull(calibrator.getEstimatedBiasFx());
10939 assertNull(calibrator.getEstimatedBiasFy());
10940 assertNull(calibrator.getEstimatedBiasFz());
10941 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10942 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10943 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10944 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10945 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10946 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10947 assertNull(calibrator.getEstimatedMa());
10948 assertNull(calibrator.getEstimatedSx());
10949 assertNull(calibrator.getEstimatedSy());
10950 assertNull(calibrator.getEstimatedSz());
10951 assertNull(calibrator.getEstimatedMxy());
10952 assertNull(calibrator.getEstimatedMxz());
10953 assertNull(calibrator.getEstimatedMyx());
10954 assertNull(calibrator.getEstimatedMyz());
10955 assertNull(calibrator.getEstimatedMzx());
10956 assertNull(calibrator.getEstimatedMzy());
10957 assertNull(calibrator.getEstimatedCovariance());
10958 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10959 }
10960
10961 @Test
10962 public void testConstructor97() throws WrongSizeException {
10963 final Collection<StandardDeviationBodyKinematics> measurements =
10964 Collections.emptyList();
10965
10966 final Matrix ba = generateBa();
10967 final double biasX = ba.getElementAtIndex(0);
10968 final double biasY = ba.getElementAtIndex(1);
10969 final double biasZ = ba.getElementAtIndex(2);
10970
10971 final Acceleration bx = new Acceleration(biasX,
10972 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10973 final Acceleration by = new Acceleration(biasY,
10974 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10975 final Acceleration bz = new Acceleration(biasZ,
10976 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10977
10978 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10979 final double latitude = Math.toRadians(
10980 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10981 final double longitude = Math.toRadians(
10982 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10983 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10984 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10985 final NEDVelocity nedVelocity = new NEDVelocity();
10986 final ECEFPosition ecefPosition = new ECEFPosition();
10987 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10988 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10989 ecefPosition, ecefVelocity);
10990
10991 final KnownPositionAccelerometerCalibrator calibrator =
10992 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
10993 bx, by, bz);
10994
10995
10996 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10997 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10998 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10999 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11000 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11001 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11002 final Acceleration bx2 = new Acceleration(0.0,
11003 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11004 calibrator.getInitialBiasXAsAcceleration(bx2);
11005 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11006 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11007 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11008 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11009 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11010 final Acceleration by2 = new Acceleration(0.0,
11011 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11012 calibrator.getInitialBiasYAsAcceleration(by2);
11013 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11014 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11015 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11016 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11017 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11018 final Acceleration bz2 = new Acceleration(0.0,
11019 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11020 calibrator.getInitialBiasZAsAcceleration(bz2);
11021 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11022 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11023 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11024 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11025 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11026 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11027 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11028 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11029 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11030 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11031 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11032 final double[] bias1 = calibrator.getInitialBias();
11033 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
11034 final double[] bias2 = new double[3];
11035 calibrator.getInitialBias(bias2);
11036 assertArrayEquals(bias1, bias2, 0.0);
11037 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11038 assertEquals(b1, ba);
11039 final Matrix b2 = new Matrix(3, 1);
11040 calibrator.getInitialBiasAsMatrix(b2);
11041 assertEquals(b1, b2);
11042 final Matrix ma1 = calibrator.getInitialMa();
11043 assertEquals(ma1, new Matrix(3, 3));
11044 final Matrix ma2 = new Matrix(3, 3);
11045 calibrator.getInitialMa(ma2);
11046 assertEquals(ma1, ma2);
11047 assertSame(calibrator.getEcefPosition(), ecefPosition);
11048 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11049 final NEDPosition nedPosition1 = new NEDPosition();
11050 assertTrue(calibrator.getNedPosition(nedPosition1));
11051 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11052 assertSame(calibrator.getMeasurements(), measurements);
11053 assertFalse(calibrator.isCommonAxisUsed());
11054 assertNull(calibrator.getListener());
11055 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11056 assertFalse(calibrator.isReady());
11057 assertFalse(calibrator.isRunning());
11058 assertNull(calibrator.getEstimatedBiases());
11059 assertFalse(calibrator.getEstimatedBiases(null));
11060 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11061 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11062 assertNull(calibrator.getEstimatedBiasFx());
11063 assertNull(calibrator.getEstimatedBiasFy());
11064 assertNull(calibrator.getEstimatedBiasFz());
11065 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11066 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11067 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11068 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11069 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11070 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11071 assertNull(calibrator.getEstimatedMa());
11072 assertNull(calibrator.getEstimatedSx());
11073 assertNull(calibrator.getEstimatedSy());
11074 assertNull(calibrator.getEstimatedSz());
11075 assertNull(calibrator.getEstimatedMxy());
11076 assertNull(calibrator.getEstimatedMxz());
11077 assertNull(calibrator.getEstimatedMyx());
11078 assertNull(calibrator.getEstimatedMyz());
11079 assertNull(calibrator.getEstimatedMzx());
11080 assertNull(calibrator.getEstimatedMzy());
11081 assertNull(calibrator.getEstimatedCovariance());
11082 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11083 }
11084
11085 @Test
11086 public void testConstructor98() throws WrongSizeException {
11087 final Collection<StandardDeviationBodyKinematics> measurements =
11088 Collections.emptyList();
11089
11090 final Matrix ba = generateBa();
11091 final double biasX = ba.getElementAtIndex(0);
11092 final double biasY = ba.getElementAtIndex(1);
11093 final double biasZ = ba.getElementAtIndex(2);
11094
11095 final Acceleration bx = new Acceleration(biasX,
11096 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11097 final Acceleration by = new Acceleration(biasY,
11098 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11099 final Acceleration bz = new Acceleration(biasZ,
11100 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11101
11102 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11103 final double latitude = Math.toRadians(
11104 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11105 final double longitude = Math.toRadians(
11106 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11107 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11108 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11109 final NEDVelocity nedVelocity = new NEDVelocity();
11110 final ECEFPosition ecefPosition = new ECEFPosition();
11111 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11112 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11113 ecefPosition, ecefVelocity);
11114
11115 final KnownPositionAccelerometerCalibrator calibrator =
11116 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
11117 bx, by, bz, this);
11118
11119
11120 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11121 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11122 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11123 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11124 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11125 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11126 final Acceleration bx2 = new Acceleration(0.0,
11127 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11128 calibrator.getInitialBiasXAsAcceleration(bx2);
11129 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11130 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11131 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11132 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11133 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11134 final Acceleration by2 = new Acceleration(0.0,
11135 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11136 calibrator.getInitialBiasYAsAcceleration(by2);
11137 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11138 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11139 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11140 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11141 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11142 final Acceleration bz2 = new Acceleration(0.0,
11143 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11144 calibrator.getInitialBiasZAsAcceleration(bz2);
11145 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11146 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11147 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11148 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11149 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11150 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11151 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11152 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11153 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11154 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11155 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11156 final double[] bias1 = calibrator.getInitialBias();
11157 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
11158 final double[] bias2 = new double[3];
11159 calibrator.getInitialBias(bias2);
11160 assertArrayEquals(bias1, bias2, 0.0);
11161 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11162 assertEquals(b1, ba);
11163 final Matrix b2 = new Matrix(3, 1);
11164 calibrator.getInitialBiasAsMatrix(b2);
11165 assertEquals(b1, b2);
11166 final Matrix ma1 = calibrator.getInitialMa();
11167 assertEquals(ma1, new Matrix(3, 3));
11168 final Matrix ma2 = new Matrix(3, 3);
11169 calibrator.getInitialMa(ma2);
11170 assertEquals(ma1, ma2);
11171 assertSame(calibrator.getEcefPosition(), ecefPosition);
11172 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11173 final NEDPosition nedPosition1 = new NEDPosition();
11174 assertTrue(calibrator.getNedPosition(nedPosition1));
11175 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11176 assertSame(calibrator.getMeasurements(), measurements);
11177 assertFalse(calibrator.isCommonAxisUsed());
11178 assertSame(calibrator.getListener(), this);
11179 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11180 assertFalse(calibrator.isReady());
11181 assertFalse(calibrator.isRunning());
11182 assertNull(calibrator.getEstimatedBiases());
11183 assertFalse(calibrator.getEstimatedBiases(null));
11184 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11185 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11186 assertNull(calibrator.getEstimatedBiasFx());
11187 assertNull(calibrator.getEstimatedBiasFy());
11188 assertNull(calibrator.getEstimatedBiasFz());
11189 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11190 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11191 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11192 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11193 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11194 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11195 assertNull(calibrator.getEstimatedMa());
11196 assertNull(calibrator.getEstimatedSx());
11197 assertNull(calibrator.getEstimatedSy());
11198 assertNull(calibrator.getEstimatedSz());
11199 assertNull(calibrator.getEstimatedMxy());
11200 assertNull(calibrator.getEstimatedMxz());
11201 assertNull(calibrator.getEstimatedMyx());
11202 assertNull(calibrator.getEstimatedMyz());
11203 assertNull(calibrator.getEstimatedMzx());
11204 assertNull(calibrator.getEstimatedMzy());
11205 assertNull(calibrator.getEstimatedCovariance());
11206 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11207 }
11208
11209 @Test
11210 public void testConstructor99() throws WrongSizeException {
11211 final Matrix ba = generateBa();
11212 final double biasX = ba.getElementAtIndex(0);
11213 final double biasY = ba.getElementAtIndex(1);
11214 final double biasZ = ba.getElementAtIndex(2);
11215
11216 final Acceleration bx = new Acceleration(biasX,
11217 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11218 final Acceleration by = new Acceleration(biasY,
11219 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11220 final Acceleration bz = new Acceleration(biasZ,
11221 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11222
11223 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11224 final double latitude = Math.toRadians(
11225 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11226 final double longitude = Math.toRadians(
11227 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11228 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11229 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11230 final NEDVelocity nedVelocity = new NEDVelocity();
11231 final ECEFPosition ecefPosition = new ECEFPosition();
11232 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11233 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11234 ecefPosition, ecefVelocity);
11235
11236 final KnownPositionAccelerometerCalibrator calibrator =
11237 new KnownPositionAccelerometerCalibrator(ecefPosition,
11238 true, bx, by, bz);
11239
11240
11241 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11242 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11243 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11244 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11245 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11246 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11247 final Acceleration bx2 = new Acceleration(0.0,
11248 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11249 calibrator.getInitialBiasXAsAcceleration(bx2);
11250 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11251 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11252 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11253 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11254 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11255 final Acceleration by2 = new Acceleration(0.0,
11256 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11257 calibrator.getInitialBiasYAsAcceleration(by2);
11258 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11259 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11260 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11261 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11262 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11263 final Acceleration bz2 = new Acceleration(0.0,
11264 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11265 calibrator.getInitialBiasZAsAcceleration(bz2);
11266 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11267 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11268 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11269 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11270 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11271 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11272 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11273 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11274 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11275 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11276 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11277 final double[] bias1 = calibrator.getInitialBias();
11278 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
11279 final double[] bias2 = new double[3];
11280 calibrator.getInitialBias(bias2);
11281 assertArrayEquals(bias1, bias2, 0.0);
11282 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11283 assertEquals(b1, ba);
11284 final Matrix b2 = new Matrix(3, 1);
11285 calibrator.getInitialBiasAsMatrix(b2);
11286 assertEquals(b1, b2);
11287 final Matrix ma1 = calibrator.getInitialMa();
11288 assertEquals(ma1, new Matrix(3, 3));
11289 final Matrix ma2 = new Matrix(3, 3);
11290 calibrator.getInitialMa(ma2);
11291 assertEquals(ma1, ma2);
11292 assertSame(calibrator.getEcefPosition(), ecefPosition);
11293 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11294 final NEDPosition nedPosition1 = new NEDPosition();
11295 assertTrue(calibrator.getNedPosition(nedPosition1));
11296 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11297 assertNull(calibrator.getMeasurements());
11298 assertTrue(calibrator.isCommonAxisUsed());
11299 assertNull(calibrator.getListener());
11300 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11301 assertFalse(calibrator.isReady());
11302 assertFalse(calibrator.isRunning());
11303 assertNull(calibrator.getEstimatedBiases());
11304 assertFalse(calibrator.getEstimatedBiases(null));
11305 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11306 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11307 assertNull(calibrator.getEstimatedBiasFx());
11308 assertNull(calibrator.getEstimatedBiasFy());
11309 assertNull(calibrator.getEstimatedBiasFz());
11310 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11311 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11312 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11313 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11314 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11315 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11316 assertNull(calibrator.getEstimatedMa());
11317 assertNull(calibrator.getEstimatedSx());
11318 assertNull(calibrator.getEstimatedSy());
11319 assertNull(calibrator.getEstimatedSz());
11320 assertNull(calibrator.getEstimatedMxy());
11321 assertNull(calibrator.getEstimatedMxz());
11322 assertNull(calibrator.getEstimatedMyx());
11323 assertNull(calibrator.getEstimatedMyz());
11324 assertNull(calibrator.getEstimatedMzx());
11325 assertNull(calibrator.getEstimatedMzy());
11326 assertNull(calibrator.getEstimatedCovariance());
11327 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11328 }
11329
11330 @Test
11331 public void testConstructor100() throws WrongSizeException {
11332 final Matrix ba = generateBa();
11333 final double biasX = ba.getElementAtIndex(0);
11334 final double biasY = ba.getElementAtIndex(1);
11335 final double biasZ = ba.getElementAtIndex(2);
11336
11337 final Acceleration bx = new Acceleration(biasX,
11338 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11339 final Acceleration by = new Acceleration(biasY,
11340 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11341 final Acceleration bz = new Acceleration(biasZ,
11342 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11343
11344 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11345 final double latitude = Math.toRadians(
11346 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11347 final double longitude = Math.toRadians(
11348 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11349 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11350 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11351 final NEDVelocity nedVelocity = new NEDVelocity();
11352 final ECEFPosition ecefPosition = new ECEFPosition();
11353 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11354 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11355 ecefPosition, ecefVelocity);
11356
11357 final KnownPositionAccelerometerCalibrator calibrator =
11358 new KnownPositionAccelerometerCalibrator(ecefPosition,
11359 true, bx, by, bz, this);
11360
11361
11362 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11363 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11364 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11365 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11366 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11367 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11368 final Acceleration bx2 = new Acceleration(0.0,
11369 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11370 calibrator.getInitialBiasXAsAcceleration(bx2);
11371 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11372 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11373 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11374 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11375 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11376 final Acceleration by2 = new Acceleration(0.0,
11377 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11378 calibrator.getInitialBiasYAsAcceleration(by2);
11379 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11380 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11381 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11382 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11383 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11384 final Acceleration bz2 = new Acceleration(0.0,
11385 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11386 calibrator.getInitialBiasZAsAcceleration(bz2);
11387 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11388 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11389 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11390 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11391 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11392 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11393 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11394 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11395 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11396 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11397 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11398 final double[] bias1 = calibrator.getInitialBias();
11399 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
11400 final double[] bias2 = new double[3];
11401 calibrator.getInitialBias(bias2);
11402 assertArrayEquals(bias1, bias2, 0.0);
11403 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11404 assertEquals(b1, ba);
11405 final Matrix b2 = new Matrix(3, 1);
11406 calibrator.getInitialBiasAsMatrix(b2);
11407 assertEquals(b1, b2);
11408 final Matrix ma1 = calibrator.getInitialMa();
11409 assertEquals(ma1, new Matrix(3, 3));
11410 final Matrix ma2 = new Matrix(3, 3);
11411 calibrator.getInitialMa(ma2);
11412 assertEquals(ma1, ma2);
11413 assertSame(calibrator.getEcefPosition(), ecefPosition);
11414 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11415 final NEDPosition nedPosition1 = new NEDPosition();
11416 assertTrue(calibrator.getNedPosition(nedPosition1));
11417 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11418 assertNull(calibrator.getMeasurements());
11419 assertTrue(calibrator.isCommonAxisUsed());
11420 assertSame(calibrator.getListener(), this);
11421 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11422 assertFalse(calibrator.isReady());
11423 assertFalse(calibrator.isRunning());
11424 assertNull(calibrator.getEstimatedBiases());
11425 assertFalse(calibrator.getEstimatedBiases(null));
11426 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11427 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11428 assertNull(calibrator.getEstimatedBiasFx());
11429 assertNull(calibrator.getEstimatedBiasFy());
11430 assertNull(calibrator.getEstimatedBiasFz());
11431 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11432 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11433 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11434 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11435 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11436 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11437 assertNull(calibrator.getEstimatedMa());
11438 assertNull(calibrator.getEstimatedSx());
11439 assertNull(calibrator.getEstimatedSy());
11440 assertNull(calibrator.getEstimatedSz());
11441 assertNull(calibrator.getEstimatedMxy());
11442 assertNull(calibrator.getEstimatedMxz());
11443 assertNull(calibrator.getEstimatedMyx());
11444 assertNull(calibrator.getEstimatedMyz());
11445 assertNull(calibrator.getEstimatedMzx());
11446 assertNull(calibrator.getEstimatedMzy());
11447 assertNull(calibrator.getEstimatedCovariance());
11448 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11449 }
11450
11451 @Test
11452 public void testConstructor101() throws WrongSizeException {
11453 final Collection<StandardDeviationBodyKinematics> measurements =
11454 Collections.emptyList();
11455
11456 final Matrix ba = generateBa();
11457 final double biasX = ba.getElementAtIndex(0);
11458 final double biasY = ba.getElementAtIndex(1);
11459 final double biasZ = ba.getElementAtIndex(2);
11460
11461 final Acceleration bx = new Acceleration(biasX,
11462 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11463 final Acceleration by = new Acceleration(biasY,
11464 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11465 final Acceleration bz = new Acceleration(biasZ,
11466 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11467
11468 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11469 final double latitude = Math.toRadians(
11470 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11471 final double longitude = Math.toRadians(
11472 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11473 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11474 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11475 final NEDVelocity nedVelocity = new NEDVelocity();
11476 final ECEFPosition ecefPosition = new ECEFPosition();
11477 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11478 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11479 ecefPosition, ecefVelocity);
11480
11481 final KnownPositionAccelerometerCalibrator calibrator =
11482 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
11483 true, bx, by, bz);
11484
11485
11486 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11487 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11488 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11489 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11490 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11491 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11492 final Acceleration bx2 = new Acceleration(0.0,
11493 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11494 calibrator.getInitialBiasXAsAcceleration(bx2);
11495 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11496 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11497 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11498 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11499 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11500 final Acceleration by2 = new Acceleration(0.0,
11501 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11502 calibrator.getInitialBiasYAsAcceleration(by2);
11503 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11504 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11505 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11506 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11507 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11508 final Acceleration bz2 = new Acceleration(0.0,
11509 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11510 calibrator.getInitialBiasZAsAcceleration(bz2);
11511 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11512 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11513 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11514 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11515 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11516 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11517 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11518 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11519 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11520 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11521 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11522 final double[] bias1 = calibrator.getInitialBias();
11523 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
11524 final double[] bias2 = new double[3];
11525 calibrator.getInitialBias(bias2);
11526 assertArrayEquals(bias1, bias2, 0.0);
11527 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11528 assertEquals(b1, ba);
11529 final Matrix b2 = new Matrix(3, 1);
11530 calibrator.getInitialBiasAsMatrix(b2);
11531 assertEquals(b1, b2);
11532 final Matrix ma1 = calibrator.getInitialMa();
11533 assertEquals(ma1, new Matrix(3, 3));
11534 final Matrix ma2 = new Matrix(3, 3);
11535 calibrator.getInitialMa(ma2);
11536 assertEquals(ma1, ma2);
11537 assertSame(calibrator.getEcefPosition(), ecefPosition);
11538 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11539 final NEDPosition nedPosition1 = new NEDPosition();
11540 assertTrue(calibrator.getNedPosition(nedPosition1));
11541 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11542 assertSame(calibrator.getMeasurements(), measurements);
11543 assertTrue(calibrator.isCommonAxisUsed());
11544 assertNull(calibrator.getListener());
11545 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11546 assertFalse(calibrator.isReady());
11547 assertFalse(calibrator.isRunning());
11548 assertNull(calibrator.getEstimatedBiases());
11549 assertFalse(calibrator.getEstimatedBiases(null));
11550 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11551 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11552 assertNull(calibrator.getEstimatedBiasFx());
11553 assertNull(calibrator.getEstimatedBiasFy());
11554 assertNull(calibrator.getEstimatedBiasFz());
11555 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11556 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11557 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11558 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11559 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11560 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11561 assertNull(calibrator.getEstimatedMa());
11562 assertNull(calibrator.getEstimatedSx());
11563 assertNull(calibrator.getEstimatedSy());
11564 assertNull(calibrator.getEstimatedSz());
11565 assertNull(calibrator.getEstimatedMxy());
11566 assertNull(calibrator.getEstimatedMxz());
11567 assertNull(calibrator.getEstimatedMyx());
11568 assertNull(calibrator.getEstimatedMyz());
11569 assertNull(calibrator.getEstimatedMzx());
11570 assertNull(calibrator.getEstimatedMzy());
11571 assertNull(calibrator.getEstimatedCovariance());
11572 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11573 }
11574
11575 @Test
11576 public void testConstructor102() throws WrongSizeException {
11577 final Collection<StandardDeviationBodyKinematics> measurements =
11578 Collections.emptyList();
11579
11580 final Matrix ba = generateBa();
11581 final double biasX = ba.getElementAtIndex(0);
11582 final double biasY = ba.getElementAtIndex(1);
11583 final double biasZ = ba.getElementAtIndex(2);
11584
11585 final Acceleration bx = new Acceleration(biasX,
11586 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11587 final Acceleration by = new Acceleration(biasY,
11588 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11589 final Acceleration bz = new Acceleration(biasZ,
11590 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11591
11592 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11593 final double latitude = Math.toRadians(
11594 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11595 final double longitude = Math.toRadians(
11596 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11597 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11598 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11599 final NEDVelocity nedVelocity = new NEDVelocity();
11600 final ECEFPosition ecefPosition = new ECEFPosition();
11601 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11602 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11603 ecefPosition, ecefVelocity);
11604
11605 final KnownPositionAccelerometerCalibrator calibrator =
11606 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
11607 true, bx, by, bz, this);
11608
11609
11610 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11611 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11612 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11613 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11614 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11615 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11616 final Acceleration bx2 = new Acceleration(0.0,
11617 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11618 calibrator.getInitialBiasXAsAcceleration(bx2);
11619 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11620 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11621 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11622 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11623 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11624 final Acceleration by2 = new Acceleration(0.0,
11625 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11626 calibrator.getInitialBiasYAsAcceleration(by2);
11627 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11628 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11629 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11630 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11631 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11632 final Acceleration bz2 = new Acceleration(0.0,
11633 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11634 calibrator.getInitialBiasZAsAcceleration(bz2);
11635 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11636 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11637 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11638 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11639 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11640 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11641 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11642 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11643 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11644 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11645 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11646 final double[] bias1 = calibrator.getInitialBias();
11647 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
11648 final double[] bias2 = new double[3];
11649 calibrator.getInitialBias(bias2);
11650 assertArrayEquals(bias1, bias2, 0.0);
11651 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11652 assertEquals(b1, ba);
11653 final Matrix b2 = new Matrix(3, 1);
11654 calibrator.getInitialBiasAsMatrix(b2);
11655 assertEquals(b1, b2);
11656 final Matrix ma1 = calibrator.getInitialMa();
11657 assertEquals(ma1, new Matrix(3, 3));
11658 final Matrix ma2 = new Matrix(3, 3);
11659 calibrator.getInitialMa(ma2);
11660 assertEquals(ma1, ma2);
11661 assertSame(calibrator.getEcefPosition(), ecefPosition);
11662 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11663 final NEDPosition nedPosition1 = new NEDPosition();
11664 assertTrue(calibrator.getNedPosition(nedPosition1));
11665 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11666 assertSame(calibrator.getMeasurements(), measurements);
11667 assertTrue(calibrator.isCommonAxisUsed());
11668 assertSame(calibrator.getListener(), this);
11669 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11670 assertFalse(calibrator.isReady());
11671 assertFalse(calibrator.isRunning());
11672 assertNull(calibrator.getEstimatedBiases());
11673 assertFalse(calibrator.getEstimatedBiases(null));
11674 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11675 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11676 assertNull(calibrator.getEstimatedBiasFx());
11677 assertNull(calibrator.getEstimatedBiasFy());
11678 assertNull(calibrator.getEstimatedBiasFz());
11679 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11680 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11681 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11682 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11683 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11684 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11685 assertNull(calibrator.getEstimatedMa());
11686 assertNull(calibrator.getEstimatedSx());
11687 assertNull(calibrator.getEstimatedSy());
11688 assertNull(calibrator.getEstimatedSz());
11689 assertNull(calibrator.getEstimatedMxy());
11690 assertNull(calibrator.getEstimatedMxz());
11691 assertNull(calibrator.getEstimatedMyx());
11692 assertNull(calibrator.getEstimatedMyz());
11693 assertNull(calibrator.getEstimatedMzx());
11694 assertNull(calibrator.getEstimatedMzy());
11695 assertNull(calibrator.getEstimatedCovariance());
11696 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11697 }
11698
11699 @Test
11700 public void testConstructor103() throws WrongSizeException {
11701 final Matrix ba = generateBa();
11702 final double biasX = ba.getElementAtIndex(0);
11703 final double biasY = ba.getElementAtIndex(1);
11704 final double biasZ = ba.getElementAtIndex(2);
11705
11706 final Matrix ma = generateMaCommonAxis();
11707 final double sx = ma.getElementAt(0, 0);
11708 final double sy = ma.getElementAt(1, 1);
11709 final double sz = ma.getElementAt(2, 2);
11710
11711 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11712 final double latitude = Math.toRadians(
11713 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11714 final double longitude = Math.toRadians(
11715 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11716 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11717 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11718 final NEDVelocity nedVelocity = new NEDVelocity();
11719 final ECEFPosition ecefPosition = new ECEFPosition();
11720 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11721 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11722 ecefPosition, ecefVelocity);
11723
11724 final KnownPositionAccelerometerCalibrator calibrator =
11725 new KnownPositionAccelerometerCalibrator(ecefPosition,
11726 biasX, biasY, biasZ, sx, sy, sz);
11727
11728
11729 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11730 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11731 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11732 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11733 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11734 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11735 final Acceleration bx2 = new Acceleration(0.0,
11736 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11737 calibrator.getInitialBiasXAsAcceleration(bx2);
11738 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11739 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11740 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11741 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11742 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11743 final Acceleration by2 = new Acceleration(0.0,
11744 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11745 calibrator.getInitialBiasYAsAcceleration(by2);
11746 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11747 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11748 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11749 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11750 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11751 final Acceleration bz2 = new Acceleration(0.0,
11752 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11753 calibrator.getInitialBiasZAsAcceleration(bz2);
11754 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11755 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11756 assertEquals(calibrator.getInitialSx(), sx, 0.0);
11757 assertEquals(calibrator.getInitialSy(), sy, 0.0);
11758 assertEquals(calibrator.getInitialSz(), sz, 0.0);
11759 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11760 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11761 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11762 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11763 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11764 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11765 final double[] bias1 = calibrator.getInitialBias();
11766 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
11767 final double[] bias2 = new double[3];
11768 calibrator.getInitialBias(bias2);
11769 assertArrayEquals(bias1, bias2, 0.0);
11770 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11771 assertEquals(b1, ba);
11772 final Matrix b2 = new Matrix(3, 1);
11773 calibrator.getInitialBiasAsMatrix(b2);
11774 assertEquals(b1, b2);
11775 final Matrix ma1 = calibrator.getInitialMa();
11776 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
11777 final Matrix ma2 = new Matrix(3, 3);
11778 calibrator.getInitialMa(ma2);
11779 assertEquals(ma1, ma2);
11780 assertSame(calibrator.getEcefPosition(), ecefPosition);
11781 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11782 final NEDPosition nedPosition1 = new NEDPosition();
11783 assertTrue(calibrator.getNedPosition(nedPosition1));
11784 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11785 assertNull(calibrator.getMeasurements());
11786 assertFalse(calibrator.isCommonAxisUsed());
11787 assertNull(calibrator.getListener());
11788 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11789 assertFalse(calibrator.isReady());
11790 assertFalse(calibrator.isRunning());
11791 assertNull(calibrator.getEstimatedBiases());
11792 assertFalse(calibrator.getEstimatedBiases(null));
11793 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11794 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11795 assertNull(calibrator.getEstimatedBiasFx());
11796 assertNull(calibrator.getEstimatedBiasFy());
11797 assertNull(calibrator.getEstimatedBiasFz());
11798 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11799 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11800 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11801 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11802 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11803 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11804 assertNull(calibrator.getEstimatedMa());
11805 assertNull(calibrator.getEstimatedSx());
11806 assertNull(calibrator.getEstimatedSy());
11807 assertNull(calibrator.getEstimatedSz());
11808 assertNull(calibrator.getEstimatedMxy());
11809 assertNull(calibrator.getEstimatedMxz());
11810 assertNull(calibrator.getEstimatedMyx());
11811 assertNull(calibrator.getEstimatedMyz());
11812 assertNull(calibrator.getEstimatedMzx());
11813 assertNull(calibrator.getEstimatedMzy());
11814 assertNull(calibrator.getEstimatedCovariance());
11815 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11816 }
11817
11818 @Test
11819 public void testConstructor104() throws WrongSizeException {
11820 final Collection<StandardDeviationBodyKinematics> measurements =
11821 Collections.emptyList();
11822
11823 final Matrix ba = generateBa();
11824 final double biasX = ba.getElementAtIndex(0);
11825 final double biasY = ba.getElementAtIndex(1);
11826 final double biasZ = ba.getElementAtIndex(2);
11827
11828 final Matrix ma = generateMaCommonAxis();
11829 final double sx = ma.getElementAt(0, 0);
11830 final double sy = ma.getElementAt(1, 1);
11831 final double sz = ma.getElementAt(2, 2);
11832
11833 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11834 final double latitude = Math.toRadians(
11835 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11836 final double longitude = Math.toRadians(
11837 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11838 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11839 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11840 final NEDVelocity nedVelocity = new NEDVelocity();
11841 final ECEFPosition ecefPosition = new ECEFPosition();
11842 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11843 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11844 ecefPosition, ecefVelocity);
11845
11846 final KnownPositionAccelerometerCalibrator calibrator =
11847 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
11848 biasX, biasY, biasZ, sx, sy, sz);
11849
11850
11851 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11852 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11853 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11854 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11855 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11856 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11857 final Acceleration bx2 = new Acceleration(0.0,
11858 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11859 calibrator.getInitialBiasXAsAcceleration(bx2);
11860 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11861 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11862 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11863 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11864 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11865 final Acceleration by2 = new Acceleration(0.0,
11866 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11867 calibrator.getInitialBiasYAsAcceleration(by2);
11868 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11869 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11870 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11871 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11872 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11873 final Acceleration bz2 = new Acceleration(0.0,
11874 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11875 calibrator.getInitialBiasZAsAcceleration(bz2);
11876 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11877 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11878 assertEquals(calibrator.getInitialSx(), sx, 0.0);
11879 assertEquals(calibrator.getInitialSy(), sy, 0.0);
11880 assertEquals(calibrator.getInitialSz(), sz, 0.0);
11881 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11882 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11883 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11884 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11885 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11886 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11887 final double[] bias1 = calibrator.getInitialBias();
11888 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
11889 final double[] bias2 = new double[3];
11890 calibrator.getInitialBias(bias2);
11891 assertArrayEquals(bias1, bias2, 0.0);
11892 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11893 assertEquals(b1, ba);
11894 final Matrix b2 = new Matrix(3, 1);
11895 calibrator.getInitialBiasAsMatrix(b2);
11896 assertEquals(b1, b2);
11897 final Matrix ma1 = calibrator.getInitialMa();
11898 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
11899 final Matrix ma2 = new Matrix(3, 3);
11900 calibrator.getInitialMa(ma2);
11901 assertEquals(ma1, ma2);
11902 assertSame(calibrator.getEcefPosition(), ecefPosition);
11903 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11904 final NEDPosition nedPosition1 = new NEDPosition();
11905 assertTrue(calibrator.getNedPosition(nedPosition1));
11906 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11907 assertSame(calibrator.getMeasurements(), measurements);
11908 assertFalse(calibrator.isCommonAxisUsed());
11909 assertNull(calibrator.getListener());
11910 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11911 assertFalse(calibrator.isReady());
11912 assertFalse(calibrator.isRunning());
11913 assertNull(calibrator.getEstimatedBiases());
11914 assertFalse(calibrator.getEstimatedBiases(null));
11915 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11916 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11917 assertNull(calibrator.getEstimatedBiasFx());
11918 assertNull(calibrator.getEstimatedBiasFy());
11919 assertNull(calibrator.getEstimatedBiasFz());
11920 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11921 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11922 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11923 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11924 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11925 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11926 assertNull(calibrator.getEstimatedMa());
11927 assertNull(calibrator.getEstimatedSx());
11928 assertNull(calibrator.getEstimatedSy());
11929 assertNull(calibrator.getEstimatedSz());
11930 assertNull(calibrator.getEstimatedMxy());
11931 assertNull(calibrator.getEstimatedMxz());
11932 assertNull(calibrator.getEstimatedMyx());
11933 assertNull(calibrator.getEstimatedMyz());
11934 assertNull(calibrator.getEstimatedMzx());
11935 assertNull(calibrator.getEstimatedMzy());
11936 assertNull(calibrator.getEstimatedCovariance());
11937 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11938 }
11939
11940 @Test
11941 public void testConstructor105() throws WrongSizeException {
11942 final Collection<StandardDeviationBodyKinematics> measurements =
11943 Collections.emptyList();
11944
11945 final Matrix ba = generateBa();
11946 final double biasX = ba.getElementAtIndex(0);
11947 final double biasY = ba.getElementAtIndex(1);
11948 final double biasZ = ba.getElementAtIndex(2);
11949
11950 final Matrix ma = generateMaCommonAxis();
11951 final double sx = ma.getElementAt(0, 0);
11952 final double sy = ma.getElementAt(1, 1);
11953 final double sz = ma.getElementAt(2, 2);
11954
11955 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11956 final double latitude = Math.toRadians(
11957 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11958 final double longitude = Math.toRadians(
11959 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11960 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11961 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11962 final NEDVelocity nedVelocity = new NEDVelocity();
11963 final ECEFPosition ecefPosition = new ECEFPosition();
11964 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11965 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11966 ecefPosition, ecefVelocity);
11967
11968 final KnownPositionAccelerometerCalibrator calibrator =
11969 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
11970 biasX, biasY, biasZ, sx, sy, sz, this);
11971
11972
11973 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11974 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11975 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11976 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11977 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11978 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11979 final Acceleration bx2 = new Acceleration(0.0,
11980 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11981 calibrator.getInitialBiasXAsAcceleration(bx2);
11982 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11983 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11984 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11985 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11986 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11987 final Acceleration by2 = new Acceleration(0.0,
11988 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11989 calibrator.getInitialBiasYAsAcceleration(by2);
11990 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11991 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11992 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11993 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11994 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11995 final Acceleration bz2 = new Acceleration(0.0,
11996 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11997 calibrator.getInitialBiasZAsAcceleration(bz2);
11998 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11999 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12000 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12001 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12002 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12003 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12004 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12005 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12006 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12007 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12008 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12009 final double[] bias1 = calibrator.getInitialBias();
12010 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
12011 final double[] bias2 = new double[3];
12012 calibrator.getInitialBias(bias2);
12013 assertArrayEquals(bias1, bias2, 0.0);
12014 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12015 assertEquals(b1, ba);
12016 final Matrix b2 = new Matrix(3, 1);
12017 calibrator.getInitialBiasAsMatrix(b2);
12018 assertEquals(b1, b2);
12019 final Matrix ma1 = calibrator.getInitialMa();
12020 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
12021 final Matrix ma2 = new Matrix(3, 3);
12022 calibrator.getInitialMa(ma2);
12023 assertEquals(ma1, ma2);
12024 assertSame(calibrator.getEcefPosition(), ecefPosition);
12025 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12026 final NEDPosition nedPosition1 = new NEDPosition();
12027 assertTrue(calibrator.getNedPosition(nedPosition1));
12028 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12029 assertSame(calibrator.getMeasurements(), measurements);
12030 assertFalse(calibrator.isCommonAxisUsed());
12031 assertSame(calibrator.getListener(), this);
12032 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
12033 assertFalse(calibrator.isReady());
12034 assertFalse(calibrator.isRunning());
12035 assertNull(calibrator.getEstimatedBiases());
12036 assertFalse(calibrator.getEstimatedBiases(null));
12037 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12038 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12039 assertNull(calibrator.getEstimatedBiasFx());
12040 assertNull(calibrator.getEstimatedBiasFy());
12041 assertNull(calibrator.getEstimatedBiasFz());
12042 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12043 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12044 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12045 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12046 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12047 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12048 assertNull(calibrator.getEstimatedMa());
12049 assertNull(calibrator.getEstimatedSx());
12050 assertNull(calibrator.getEstimatedSy());
12051 assertNull(calibrator.getEstimatedSz());
12052 assertNull(calibrator.getEstimatedMxy());
12053 assertNull(calibrator.getEstimatedMxz());
12054 assertNull(calibrator.getEstimatedMyx());
12055 assertNull(calibrator.getEstimatedMyz());
12056 assertNull(calibrator.getEstimatedMzx());
12057 assertNull(calibrator.getEstimatedMzy());
12058 assertNull(calibrator.getEstimatedCovariance());
12059 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12060 }
12061
12062 @Test
12063 public void testConstructor106() throws WrongSizeException {
12064 final Matrix ba = generateBa();
12065 final double biasX = ba.getElementAtIndex(0);
12066 final double biasY = ba.getElementAtIndex(1);
12067 final double biasZ = ba.getElementAtIndex(2);
12068
12069 final Matrix ma = generateMaCommonAxis();
12070 final double sx = ma.getElementAt(0, 0);
12071 final double sy = ma.getElementAt(1, 1);
12072 final double sz = ma.getElementAt(2, 2);
12073
12074 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12075 final double latitude = Math.toRadians(
12076 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12077 final double longitude = Math.toRadians(
12078 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12079 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12080 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12081 final NEDVelocity nedVelocity = new NEDVelocity();
12082 final ECEFPosition ecefPosition = new ECEFPosition();
12083 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12084 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12085 ecefPosition, ecefVelocity);
12086
12087 final KnownPositionAccelerometerCalibrator calibrator =
12088 new KnownPositionAccelerometerCalibrator(ecefPosition,
12089 true, biasX, biasY, biasZ, sx, sy, sz);
12090
12091
12092 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12093 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12094 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12095 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12096 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12097 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12098 final Acceleration bx2 = new Acceleration(0.0,
12099 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12100 calibrator.getInitialBiasXAsAcceleration(bx2);
12101 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12102 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12103 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12104 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12105 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12106 final Acceleration by2 = new Acceleration(0.0,
12107 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12108 calibrator.getInitialBiasYAsAcceleration(by2);
12109 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12110 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12111 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12112 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12113 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12114 final Acceleration bz2 = new Acceleration(0.0,
12115 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12116 calibrator.getInitialBiasZAsAcceleration(bz2);
12117 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12118 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12119 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12120 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12121 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12122 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12123 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12124 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12125 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12126 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12127 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12128 final double[] bias1 = calibrator.getInitialBias();
12129 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
12130 final double[] bias2 = new double[3];
12131 calibrator.getInitialBias(bias2);
12132 assertArrayEquals(bias1, bias2, 0.0);
12133 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12134 assertEquals(b1, ba);
12135 final Matrix b2 = new Matrix(3, 1);
12136 calibrator.getInitialBiasAsMatrix(b2);
12137 assertEquals(b1, b2);
12138 final Matrix ma1 = calibrator.getInitialMa();
12139 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
12140 final Matrix ma2 = new Matrix(3, 3);
12141 calibrator.getInitialMa(ma2);
12142 assertEquals(ma1, ma2);
12143 assertSame(calibrator.getEcefPosition(), ecefPosition);
12144 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12145 final NEDPosition nedPosition1 = new NEDPosition();
12146 assertTrue(calibrator.getNedPosition(nedPosition1));
12147 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12148 assertNull(calibrator.getMeasurements());
12149 assertTrue(calibrator.isCommonAxisUsed());
12150 assertNull(calibrator.getListener());
12151 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12152 assertFalse(calibrator.isReady());
12153 assertFalse(calibrator.isRunning());
12154 assertNull(calibrator.getEstimatedBiases());
12155 assertFalse(calibrator.getEstimatedBiases(null));
12156 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12157 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12158 assertNull(calibrator.getEstimatedBiasFx());
12159 assertNull(calibrator.getEstimatedBiasFy());
12160 assertNull(calibrator.getEstimatedBiasFz());
12161 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12162 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12163 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12164 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12165 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12166 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12167 assertNull(calibrator.getEstimatedMa());
12168 assertNull(calibrator.getEstimatedSx());
12169 assertNull(calibrator.getEstimatedSy());
12170 assertNull(calibrator.getEstimatedSz());
12171 assertNull(calibrator.getEstimatedMxy());
12172 assertNull(calibrator.getEstimatedMxz());
12173 assertNull(calibrator.getEstimatedMyx());
12174 assertNull(calibrator.getEstimatedMyz());
12175 assertNull(calibrator.getEstimatedMzx());
12176 assertNull(calibrator.getEstimatedMzy());
12177 assertNull(calibrator.getEstimatedCovariance());
12178 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12179 }
12180
12181 @Test
12182 public void testConstructor107() throws WrongSizeException {
12183 final Matrix ba = generateBa();
12184 final double biasX = ba.getElementAtIndex(0);
12185 final double biasY = ba.getElementAtIndex(1);
12186 final double biasZ = ba.getElementAtIndex(2);
12187
12188 final Matrix ma = generateMaCommonAxis();
12189 final double sx = ma.getElementAt(0, 0);
12190 final double sy = ma.getElementAt(1, 1);
12191 final double sz = ma.getElementAt(2, 2);
12192
12193 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12194 final double latitude = Math.toRadians(
12195 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12196 final double longitude = Math.toRadians(
12197 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12198 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12199 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12200 final NEDVelocity nedVelocity = new NEDVelocity();
12201 final ECEFPosition ecefPosition = new ECEFPosition();
12202 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12203 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12204 ecefPosition, ecefVelocity);
12205
12206 final KnownPositionAccelerometerCalibrator calibrator =
12207 new KnownPositionAccelerometerCalibrator(ecefPosition,
12208 true, biasX, biasY, biasZ, sx, sy, sz,
12209 this);
12210
12211
12212 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12213 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12214 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12215 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12216 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12217 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12218 final Acceleration bx2 = new Acceleration(0.0,
12219 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12220 calibrator.getInitialBiasXAsAcceleration(bx2);
12221 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12222 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12223 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12224 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12225 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12226 final Acceleration by2 = new Acceleration(0.0,
12227 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12228 calibrator.getInitialBiasYAsAcceleration(by2);
12229 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12230 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12231 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12232 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12233 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12234 final Acceleration bz2 = new Acceleration(0.0,
12235 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12236 calibrator.getInitialBiasZAsAcceleration(bz2);
12237 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12238 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12239 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12240 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12241 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12242 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12243 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12244 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12245 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12246 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12247 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12248 final double[] bias1 = calibrator.getInitialBias();
12249 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
12250 final double[] bias2 = new double[3];
12251 calibrator.getInitialBias(bias2);
12252 assertArrayEquals(bias1, bias2, 0.0);
12253 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12254 assertEquals(b1, ba);
12255 final Matrix b2 = new Matrix(3, 1);
12256 calibrator.getInitialBiasAsMatrix(b2);
12257 assertEquals(b1, b2);
12258 final Matrix ma1 = calibrator.getInitialMa();
12259 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
12260 final Matrix ma2 = new Matrix(3, 3);
12261 calibrator.getInitialMa(ma2);
12262 assertEquals(ma1, ma2);
12263 assertSame(calibrator.getEcefPosition(), ecefPosition);
12264 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12265 final NEDPosition nedPosition1 = new NEDPosition();
12266 assertTrue(calibrator.getNedPosition(nedPosition1));
12267 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12268 assertNull(calibrator.getMeasurements());
12269 assertTrue(calibrator.isCommonAxisUsed());
12270 assertSame(calibrator.getListener(), this);
12271 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12272 assertFalse(calibrator.isReady());
12273 assertFalse(calibrator.isRunning());
12274 assertNull(calibrator.getEstimatedBiases());
12275 assertFalse(calibrator.getEstimatedBiases(null));
12276 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12277 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12278 assertNull(calibrator.getEstimatedBiasFx());
12279 assertNull(calibrator.getEstimatedBiasFy());
12280 assertNull(calibrator.getEstimatedBiasFz());
12281 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12282 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12283 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12284 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12285 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12286 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12287 assertNull(calibrator.getEstimatedMa());
12288 assertNull(calibrator.getEstimatedSx());
12289 assertNull(calibrator.getEstimatedSy());
12290 assertNull(calibrator.getEstimatedSz());
12291 assertNull(calibrator.getEstimatedMxy());
12292 assertNull(calibrator.getEstimatedMxz());
12293 assertNull(calibrator.getEstimatedMyx());
12294 assertNull(calibrator.getEstimatedMyz());
12295 assertNull(calibrator.getEstimatedMzx());
12296 assertNull(calibrator.getEstimatedMzy());
12297 assertNull(calibrator.getEstimatedCovariance());
12298 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12299 }
12300
12301 @Test
12302 public void testConstructor108() throws WrongSizeException {
12303 final Collection<StandardDeviationBodyKinematics> measurements =
12304 Collections.emptyList();
12305
12306 final Matrix ba = generateBa();
12307 final double biasX = ba.getElementAtIndex(0);
12308 final double biasY = ba.getElementAtIndex(1);
12309 final double biasZ = ba.getElementAtIndex(2);
12310
12311 final Matrix ma = generateMaCommonAxis();
12312 final double sx = ma.getElementAt(0, 0);
12313 final double sy = ma.getElementAt(1, 1);
12314 final double sz = ma.getElementAt(2, 2);
12315
12316 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12317 final double latitude = Math.toRadians(
12318 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12319 final double longitude = Math.toRadians(
12320 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12321 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12322 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12323 final NEDVelocity nedVelocity = new NEDVelocity();
12324 final ECEFPosition ecefPosition = new ECEFPosition();
12325 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12326 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12327 ecefPosition, ecefVelocity);
12328
12329 final KnownPositionAccelerometerCalibrator calibrator =
12330 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
12331 true, biasX, biasY, biasZ, sx, sy, sz);
12332
12333
12334 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12335 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12336 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12337 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12338 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12339 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12340 final Acceleration bx2 = new Acceleration(0.0,
12341 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12342 calibrator.getInitialBiasXAsAcceleration(bx2);
12343 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12344 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12345 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12346 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12347 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12348 final Acceleration by2 = new Acceleration(0.0,
12349 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12350 calibrator.getInitialBiasYAsAcceleration(by2);
12351 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12352 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12353 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12354 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12355 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12356 final Acceleration bz2 = new Acceleration(0.0,
12357 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12358 calibrator.getInitialBiasZAsAcceleration(bz2);
12359 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12360 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12361 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12362 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12363 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12364 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12365 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12366 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12367 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12368 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12369 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12370 final double[] bias1 = calibrator.getInitialBias();
12371 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
12372 final double[] bias2 = new double[3];
12373 calibrator.getInitialBias(bias2);
12374 assertArrayEquals(bias1, bias2, 0.0);
12375 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12376 assertEquals(b1, ba);
12377 final Matrix b2 = new Matrix(3, 1);
12378 calibrator.getInitialBiasAsMatrix(b2);
12379 assertEquals(b1, b2);
12380 final Matrix ma1 = calibrator.getInitialMa();
12381 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
12382 final Matrix ma2 = new Matrix(3, 3);
12383 calibrator.getInitialMa(ma2);
12384 assertEquals(ma1, ma2);
12385 assertSame(calibrator.getEcefPosition(), ecefPosition);
12386 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12387 final NEDPosition nedPosition1 = new NEDPosition();
12388 assertTrue(calibrator.getNedPosition(nedPosition1));
12389 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12390 assertSame(calibrator.getMeasurements(), measurements);
12391 assertTrue(calibrator.isCommonAxisUsed());
12392 assertNull(calibrator.getListener());
12393 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12394 assertFalse(calibrator.isReady());
12395 assertFalse(calibrator.isRunning());
12396 assertNull(calibrator.getEstimatedBiases());
12397 assertFalse(calibrator.getEstimatedBiases(null));
12398 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12399 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12400 assertNull(calibrator.getEstimatedBiasFx());
12401 assertNull(calibrator.getEstimatedBiasFy());
12402 assertNull(calibrator.getEstimatedBiasFz());
12403 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12404 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12405 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12406 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12407 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12408 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12409 assertNull(calibrator.getEstimatedMa());
12410 assertNull(calibrator.getEstimatedSx());
12411 assertNull(calibrator.getEstimatedSy());
12412 assertNull(calibrator.getEstimatedSz());
12413 assertNull(calibrator.getEstimatedMxy());
12414 assertNull(calibrator.getEstimatedMxz());
12415 assertNull(calibrator.getEstimatedMyx());
12416 assertNull(calibrator.getEstimatedMyz());
12417 assertNull(calibrator.getEstimatedMzx());
12418 assertNull(calibrator.getEstimatedMzy());
12419 assertNull(calibrator.getEstimatedCovariance());
12420 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12421 }
12422
12423 @Test
12424 public void testConstructor109() throws WrongSizeException {
12425 final Collection<StandardDeviationBodyKinematics> measurements =
12426 Collections.emptyList();
12427
12428 final Matrix ba = generateBa();
12429 final double biasX = ba.getElementAtIndex(0);
12430 final double biasY = ba.getElementAtIndex(1);
12431 final double biasZ = ba.getElementAtIndex(2);
12432
12433 final Matrix ma = generateMaCommonAxis();
12434 final double sx = ma.getElementAt(0, 0);
12435 final double sy = ma.getElementAt(1, 1);
12436 final double sz = ma.getElementAt(2, 2);
12437
12438 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12439 final double latitude = Math.toRadians(
12440 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12441 final double longitude = Math.toRadians(
12442 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12443 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12444 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12445 final NEDVelocity nedVelocity = new NEDVelocity();
12446 final ECEFPosition ecefPosition = new ECEFPosition();
12447 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12448 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12449 ecefPosition, ecefVelocity);
12450
12451 final KnownPositionAccelerometerCalibrator calibrator =
12452 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
12453 true, biasX, biasY, biasZ, sx, sy, sz,
12454 this);
12455
12456
12457 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12458 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12459 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12460 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12461 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12462 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12463 final Acceleration bx2 = new Acceleration(0.0,
12464 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12465 calibrator.getInitialBiasXAsAcceleration(bx2);
12466 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12467 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12468 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12469 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12470 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12471 final Acceleration by2 = new Acceleration(0.0,
12472 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12473 calibrator.getInitialBiasYAsAcceleration(by2);
12474 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12475 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12476 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12477 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12478 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12479 final Acceleration bz2 = new Acceleration(0.0,
12480 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12481 calibrator.getInitialBiasZAsAcceleration(bz2);
12482 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12483 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12484 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12485 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12486 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12487 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12488 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12489 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12490 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12491 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12492 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12493 final double[] bias1 = calibrator.getInitialBias();
12494 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
12495 final double[] bias2 = new double[3];
12496 calibrator.getInitialBias(bias2);
12497 assertArrayEquals(bias1, bias2, 0.0);
12498 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12499 assertEquals(b1, ba);
12500 final Matrix b2 = new Matrix(3, 1);
12501 calibrator.getInitialBiasAsMatrix(b2);
12502 assertEquals(b1, b2);
12503 final Matrix ma1 = calibrator.getInitialMa();
12504 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
12505 final Matrix ma2 = new Matrix(3, 3);
12506 calibrator.getInitialMa(ma2);
12507 assertEquals(ma1, ma2);
12508 assertSame(calibrator.getEcefPosition(), ecefPosition);
12509 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12510 final NEDPosition nedPosition1 = new NEDPosition();
12511 assertTrue(calibrator.getNedPosition(nedPosition1));
12512 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12513 assertSame(calibrator.getMeasurements(), measurements);
12514 assertTrue(calibrator.isCommonAxisUsed());
12515 assertSame(calibrator.getListener(), this);
12516 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12517 assertFalse(calibrator.isReady());
12518 assertFalse(calibrator.isRunning());
12519 assertNull(calibrator.getEstimatedBiases());
12520 assertFalse(calibrator.getEstimatedBiases(null));
12521 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12522 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12523 assertNull(calibrator.getEstimatedBiasFx());
12524 assertNull(calibrator.getEstimatedBiasFy());
12525 assertNull(calibrator.getEstimatedBiasFz());
12526 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12527 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12528 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12529 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12530 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12531 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12532 assertNull(calibrator.getEstimatedMa());
12533 assertNull(calibrator.getEstimatedSx());
12534 assertNull(calibrator.getEstimatedSy());
12535 assertNull(calibrator.getEstimatedSz());
12536 assertNull(calibrator.getEstimatedMxy());
12537 assertNull(calibrator.getEstimatedMxz());
12538 assertNull(calibrator.getEstimatedMyx());
12539 assertNull(calibrator.getEstimatedMyz());
12540 assertNull(calibrator.getEstimatedMzx());
12541 assertNull(calibrator.getEstimatedMzy());
12542 assertNull(calibrator.getEstimatedCovariance());
12543 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12544 }
12545
12546 @Test
12547 public void testConstructor110() throws WrongSizeException {
12548 final Matrix ba = generateBa();
12549 final double biasX = ba.getElementAtIndex(0);
12550 final double biasY = ba.getElementAtIndex(1);
12551 final double biasZ = ba.getElementAtIndex(2);
12552
12553 final Acceleration bx = new Acceleration(biasX,
12554 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12555 final Acceleration by = new Acceleration(biasY,
12556 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12557 final Acceleration bz = new Acceleration(biasZ,
12558 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12559
12560 final Matrix ma = generateMaCommonAxis();
12561 final double sx = ma.getElementAt(0, 0);
12562 final double sy = ma.getElementAt(1, 1);
12563 final double sz = ma.getElementAt(2, 2);
12564
12565 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12566 final double latitude = Math.toRadians(
12567 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12568 final double longitude = Math.toRadians(
12569 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12570 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12571 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12572 final NEDVelocity nedVelocity = new NEDVelocity();
12573 final ECEFPosition ecefPosition = new ECEFPosition();
12574 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12575 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12576 ecefPosition, ecefVelocity);
12577
12578 final KnownPositionAccelerometerCalibrator calibrator =
12579 new KnownPositionAccelerometerCalibrator(ecefPosition,
12580 bx, by, bz, sx, sy, sz);
12581
12582
12583 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12584 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12585 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12586 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12587 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12588 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12589 final Acceleration bx2 = new Acceleration(0.0,
12590 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12591 calibrator.getInitialBiasXAsAcceleration(bx2);
12592 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12593 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12594 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12595 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12596 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12597 final Acceleration by2 = new Acceleration(0.0,
12598 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12599 calibrator.getInitialBiasYAsAcceleration(by2);
12600 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12601 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12602 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12603 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12604 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12605 final Acceleration bz2 = new Acceleration(0.0,
12606 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12607 calibrator.getInitialBiasZAsAcceleration(bz2);
12608 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12609 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12610 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12611 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12612 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12613 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12614 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12615 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12616 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12617 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12618 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12619 final double[] bias1 = calibrator.getInitialBias();
12620 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
12621 final double[] bias2 = new double[3];
12622 calibrator.getInitialBias(bias2);
12623 assertArrayEquals(bias1, bias2, 0.0);
12624 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12625 assertEquals(b1, ba);
12626 final Matrix b2 = new Matrix(3, 1);
12627 calibrator.getInitialBiasAsMatrix(b2);
12628 assertEquals(b1, b2);
12629 final Matrix ma1 = calibrator.getInitialMa();
12630 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
12631 final Matrix ma2 = new Matrix(3, 3);
12632 calibrator.getInitialMa(ma2);
12633 assertEquals(ma1, ma2);
12634 assertSame(calibrator.getEcefPosition(), ecefPosition);
12635 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12636 final NEDPosition nedPosition1 = new NEDPosition();
12637 assertTrue(calibrator.getNedPosition(nedPosition1));
12638 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12639 assertNull(calibrator.getMeasurements());
12640 assertFalse(calibrator.isCommonAxisUsed());
12641 assertNull(calibrator.getListener());
12642 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
12643 assertFalse(calibrator.isReady());
12644 assertFalse(calibrator.isRunning());
12645 assertNull(calibrator.getEstimatedBiases());
12646 assertFalse(calibrator.getEstimatedBiases(null));
12647 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12648 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12649 assertNull(calibrator.getEstimatedBiasFx());
12650 assertNull(calibrator.getEstimatedBiasFy());
12651 assertNull(calibrator.getEstimatedBiasFz());
12652 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12653 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12654 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12655 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12656 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12657 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12658 assertNull(calibrator.getEstimatedMa());
12659 assertNull(calibrator.getEstimatedSx());
12660 assertNull(calibrator.getEstimatedSy());
12661 assertNull(calibrator.getEstimatedSz());
12662 assertNull(calibrator.getEstimatedMxy());
12663 assertNull(calibrator.getEstimatedMxz());
12664 assertNull(calibrator.getEstimatedMyx());
12665 assertNull(calibrator.getEstimatedMyz());
12666 assertNull(calibrator.getEstimatedMzx());
12667 assertNull(calibrator.getEstimatedMzy());
12668 assertNull(calibrator.getEstimatedCovariance());
12669 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12670 }
12671
12672 @Test
12673 public void testConstructor111() throws WrongSizeException {
12674 final Matrix ba = generateBa();
12675 final double biasX = ba.getElementAtIndex(0);
12676 final double biasY = ba.getElementAtIndex(1);
12677 final double biasZ = ba.getElementAtIndex(2);
12678
12679 final Acceleration bx = new Acceleration(biasX,
12680 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12681 final Acceleration by = new Acceleration(biasY,
12682 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12683 final Acceleration bz = new Acceleration(biasZ,
12684 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12685
12686 final Matrix ma = generateMaCommonAxis();
12687 final double sx = ma.getElementAt(0, 0);
12688 final double sy = ma.getElementAt(1, 1);
12689 final double sz = ma.getElementAt(2, 2);
12690
12691 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12692 final double latitude = Math.toRadians(
12693 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12694 final double longitude = Math.toRadians(
12695 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12696 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12697 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12698 final NEDVelocity nedVelocity = new NEDVelocity();
12699 final ECEFPosition ecefPosition = new ECEFPosition();
12700 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12701 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12702 ecefPosition, ecefVelocity);
12703
12704 final KnownPositionAccelerometerCalibrator calibrator =
12705 new KnownPositionAccelerometerCalibrator(ecefPosition,
12706 bx, by, bz, sx, sy, sz, this);
12707
12708
12709 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12710 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12711 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12712 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12713 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12714 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12715 final Acceleration bx2 = new Acceleration(0.0,
12716 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12717 calibrator.getInitialBiasXAsAcceleration(bx2);
12718 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12719 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12720 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12721 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12722 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12723 final Acceleration by2 = new Acceleration(0.0,
12724 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12725 calibrator.getInitialBiasYAsAcceleration(by2);
12726 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12727 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12728 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12729 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12730 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12731 final Acceleration bz2 = new Acceleration(0.0,
12732 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12733 calibrator.getInitialBiasZAsAcceleration(bz2);
12734 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12735 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12736 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12737 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12738 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12739 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12740 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12741 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12742 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12743 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12744 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12745 final double[] bias1 = calibrator.getInitialBias();
12746 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
12747 final double[] bias2 = new double[3];
12748 calibrator.getInitialBias(bias2);
12749 assertArrayEquals(bias1, bias2, 0.0);
12750 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12751 assertEquals(b1, ba);
12752 final Matrix b2 = new Matrix(3, 1);
12753 calibrator.getInitialBiasAsMatrix(b2);
12754 assertEquals(b1, b2);
12755 final Matrix ma1 = calibrator.getInitialMa();
12756 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
12757 final Matrix ma2 = new Matrix(3, 3);
12758 calibrator.getInitialMa(ma2);
12759 assertEquals(ma1, ma2);
12760 assertSame(calibrator.getEcefPosition(), ecefPosition);
12761 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12762 final NEDPosition nedPosition1 = new NEDPosition();
12763 assertTrue(calibrator.getNedPosition(nedPosition1));
12764 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12765 assertNull(calibrator.getMeasurements());
12766 assertFalse(calibrator.isCommonAxisUsed());
12767 assertSame(calibrator.getListener(), this);
12768 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
12769 assertFalse(calibrator.isReady());
12770 assertFalse(calibrator.isRunning());
12771 assertNull(calibrator.getEstimatedBiases());
12772 assertFalse(calibrator.getEstimatedBiases(null));
12773 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12774 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12775 assertNull(calibrator.getEstimatedBiasFx());
12776 assertNull(calibrator.getEstimatedBiasFy());
12777 assertNull(calibrator.getEstimatedBiasFz());
12778 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12779 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12780 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12781 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12782 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12783 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12784 assertNull(calibrator.getEstimatedMa());
12785 assertNull(calibrator.getEstimatedSx());
12786 assertNull(calibrator.getEstimatedSy());
12787 assertNull(calibrator.getEstimatedSz());
12788 assertNull(calibrator.getEstimatedMxy());
12789 assertNull(calibrator.getEstimatedMxz());
12790 assertNull(calibrator.getEstimatedMyx());
12791 assertNull(calibrator.getEstimatedMyz());
12792 assertNull(calibrator.getEstimatedMzx());
12793 assertNull(calibrator.getEstimatedMzy());
12794 assertNull(calibrator.getEstimatedCovariance());
12795 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12796 }
12797
12798 @Test
12799 public void testConstructor112() throws WrongSizeException {
12800 final Collection<StandardDeviationBodyKinematics> measurements =
12801 Collections.emptyList();
12802
12803 final Matrix ba = generateBa();
12804 final double biasX = ba.getElementAtIndex(0);
12805 final double biasY = ba.getElementAtIndex(1);
12806 final double biasZ = ba.getElementAtIndex(2);
12807
12808 final Acceleration bx = new Acceleration(biasX,
12809 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12810 final Acceleration by = new Acceleration(biasY,
12811 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12812 final Acceleration bz = new Acceleration(biasZ,
12813 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12814
12815 final Matrix ma = generateMaCommonAxis();
12816 final double sx = ma.getElementAt(0, 0);
12817 final double sy = ma.getElementAt(1, 1);
12818 final double sz = ma.getElementAt(2, 2);
12819
12820 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12821 final double latitude = Math.toRadians(
12822 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12823 final double longitude = Math.toRadians(
12824 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12825 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12826 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12827 final NEDVelocity nedVelocity = new NEDVelocity();
12828 final ECEFPosition ecefPosition = new ECEFPosition();
12829 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12830 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12831 ecefPosition, ecefVelocity);
12832
12833 final KnownPositionAccelerometerCalibrator calibrator =
12834 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
12835 bx, by, bz, sx, sy, sz);
12836
12837
12838 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12839 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12840 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12841 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12842 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12843 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12844 final Acceleration bx2 = new Acceleration(0.0,
12845 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12846 calibrator.getInitialBiasXAsAcceleration(bx2);
12847 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12848 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12849 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12850 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12851 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12852 final Acceleration by2 = new Acceleration(0.0,
12853 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12854 calibrator.getInitialBiasYAsAcceleration(by2);
12855 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12856 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12857 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12858 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12859 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12860 final Acceleration bz2 = new Acceleration(0.0,
12861 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12862 calibrator.getInitialBiasZAsAcceleration(bz2);
12863 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12864 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12865 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12866 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12867 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12868 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12869 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12870 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12871 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12872 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12873 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12874 final double[] bias1 = calibrator.getInitialBias();
12875 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
12876 final double[] bias2 = new double[3];
12877 calibrator.getInitialBias(bias2);
12878 assertArrayEquals(bias1, bias2, 0.0);
12879 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12880 assertEquals(b1, ba);
12881 final Matrix b2 = new Matrix(3, 1);
12882 calibrator.getInitialBiasAsMatrix(b2);
12883 assertEquals(b1, b2);
12884 final Matrix ma1 = calibrator.getInitialMa();
12885 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
12886 final Matrix ma2 = new Matrix(3, 3);
12887 calibrator.getInitialMa(ma2);
12888 assertEquals(ma1, ma2);
12889 assertSame(calibrator.getEcefPosition(), ecefPosition);
12890 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12891 final NEDPosition nedPosition1 = new NEDPosition();
12892 assertTrue(calibrator.getNedPosition(nedPosition1));
12893 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12894 assertSame(calibrator.getMeasurements(), measurements);
12895 assertFalse(calibrator.isCommonAxisUsed());
12896 assertNull(calibrator.getListener());
12897 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
12898 assertFalse(calibrator.isReady());
12899 assertFalse(calibrator.isRunning());
12900 assertNull(calibrator.getEstimatedBiases());
12901 assertFalse(calibrator.getEstimatedBiases(null));
12902 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12903 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12904 assertNull(calibrator.getEstimatedBiasFx());
12905 assertNull(calibrator.getEstimatedBiasFy());
12906 assertNull(calibrator.getEstimatedBiasFz());
12907 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12908 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12909 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12910 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12911 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12912 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12913 assertNull(calibrator.getEstimatedMa());
12914 assertNull(calibrator.getEstimatedSx());
12915 assertNull(calibrator.getEstimatedSy());
12916 assertNull(calibrator.getEstimatedSz());
12917 assertNull(calibrator.getEstimatedMxy());
12918 assertNull(calibrator.getEstimatedMxz());
12919 assertNull(calibrator.getEstimatedMyx());
12920 assertNull(calibrator.getEstimatedMyz());
12921 assertNull(calibrator.getEstimatedMzx());
12922 assertNull(calibrator.getEstimatedMzy());
12923 assertNull(calibrator.getEstimatedCovariance());
12924 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12925 }
12926
12927 @Test
12928 public void testConstructor113() throws WrongSizeException {
12929 final Collection<StandardDeviationBodyKinematics> measurements =
12930 Collections.emptyList();
12931
12932 final Matrix ba = generateBa();
12933 final double biasX = ba.getElementAtIndex(0);
12934 final double biasY = ba.getElementAtIndex(1);
12935 final double biasZ = ba.getElementAtIndex(2);
12936
12937 final Acceleration bx = new Acceleration(biasX,
12938 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12939 final Acceleration by = new Acceleration(biasY,
12940 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12941 final Acceleration bz = new Acceleration(biasZ,
12942 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12943
12944 final Matrix ma = generateMaCommonAxis();
12945 final double sx = ma.getElementAt(0, 0);
12946 final double sy = ma.getElementAt(1, 1);
12947 final double sz = ma.getElementAt(2, 2);
12948
12949 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12950 final double latitude = Math.toRadians(
12951 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12952 final double longitude = Math.toRadians(
12953 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12954 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12955 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12956 final NEDVelocity nedVelocity = new NEDVelocity();
12957 final ECEFPosition ecefPosition = new ECEFPosition();
12958 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12959 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12960 ecefPosition, ecefVelocity);
12961
12962 final KnownPositionAccelerometerCalibrator calibrator =
12963 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
12964 bx, by, bz, sx, sy, sz, this);
12965
12966
12967 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12968 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12969 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12970 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12971 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12972 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12973 final Acceleration bx2 = new Acceleration(0.0,
12974 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12975 calibrator.getInitialBiasXAsAcceleration(bx2);
12976 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12977 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12978 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12979 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12980 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12981 final Acceleration by2 = new Acceleration(0.0,
12982 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12983 calibrator.getInitialBiasYAsAcceleration(by2);
12984 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12985 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12986 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12987 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12988 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12989 final Acceleration bz2 = new Acceleration(0.0,
12990 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12991 calibrator.getInitialBiasZAsAcceleration(bz2);
12992 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12993 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12994 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12995 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12996 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12997 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12998 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12999 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13000 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13001 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13002 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13003 final double[] bias1 = calibrator.getInitialBias();
13004 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
13005 final double[] bias2 = new double[3];
13006 calibrator.getInitialBias(bias2);
13007 assertArrayEquals(bias1, bias2, 0.0);
13008 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13009 assertEquals(b1, ba);
13010 final Matrix b2 = new Matrix(3, 1);
13011 calibrator.getInitialBiasAsMatrix(b2);
13012 assertEquals(b1, b2);
13013 final Matrix ma1 = calibrator.getInitialMa();
13014 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
13015 final Matrix ma2 = new Matrix(3, 3);
13016 calibrator.getInitialMa(ma2);
13017 assertEquals(ma1, ma2);
13018 assertSame(calibrator.getEcefPosition(), ecefPosition);
13019 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13020 final NEDPosition nedPosition1 = new NEDPosition();
13021 assertTrue(calibrator.getNedPosition(nedPosition1));
13022 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13023 assertSame(calibrator.getMeasurements(), measurements);
13024 assertFalse(calibrator.isCommonAxisUsed());
13025 assertSame(calibrator.getListener(), this);
13026 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13027 assertFalse(calibrator.isReady());
13028 assertFalse(calibrator.isRunning());
13029 assertNull(calibrator.getEstimatedBiases());
13030 assertFalse(calibrator.getEstimatedBiases(null));
13031 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13032 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13033 assertNull(calibrator.getEstimatedBiasFx());
13034 assertNull(calibrator.getEstimatedBiasFy());
13035 assertNull(calibrator.getEstimatedBiasFz());
13036 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13037 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13038 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13039 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13040 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13041 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13042 assertNull(calibrator.getEstimatedMa());
13043 assertNull(calibrator.getEstimatedSx());
13044 assertNull(calibrator.getEstimatedSy());
13045 assertNull(calibrator.getEstimatedSz());
13046 assertNull(calibrator.getEstimatedMxy());
13047 assertNull(calibrator.getEstimatedMxz());
13048 assertNull(calibrator.getEstimatedMyx());
13049 assertNull(calibrator.getEstimatedMyz());
13050 assertNull(calibrator.getEstimatedMzx());
13051 assertNull(calibrator.getEstimatedMzy());
13052 assertNull(calibrator.getEstimatedCovariance());
13053 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13054 }
13055
13056 @Test
13057 public void testConstructor114() throws WrongSizeException {
13058 final Matrix ba = generateBa();
13059 final double biasX = ba.getElementAtIndex(0);
13060 final double biasY = ba.getElementAtIndex(1);
13061 final double biasZ = ba.getElementAtIndex(2);
13062
13063 final Acceleration bx = new Acceleration(biasX,
13064 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13065 final Acceleration by = new Acceleration(biasY,
13066 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13067 final Acceleration bz = new Acceleration(biasZ,
13068 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13069
13070 final Matrix ma = generateMaCommonAxis();
13071 final double sx = ma.getElementAt(0, 0);
13072 final double sy = ma.getElementAt(1, 1);
13073 final double sz = ma.getElementAt(2, 2);
13074
13075 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13076 final double latitude = Math.toRadians(
13077 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13078 final double longitude = Math.toRadians(
13079 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13080 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13081 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13082 final NEDVelocity nedVelocity = new NEDVelocity();
13083 final ECEFPosition ecefPosition = new ECEFPosition();
13084 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13085 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13086 ecefPosition, ecefVelocity);
13087
13088 final KnownPositionAccelerometerCalibrator calibrator =
13089 new KnownPositionAccelerometerCalibrator(ecefPosition,
13090 true, bx, by, bz, sx, sy, sz);
13091
13092
13093 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13094 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13095 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13096 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13097 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13098 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13099 final Acceleration bx2 = new Acceleration(0.0,
13100 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13101 calibrator.getInitialBiasXAsAcceleration(bx2);
13102 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13103 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13104 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13105 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13106 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13107 final Acceleration by2 = new Acceleration(0.0,
13108 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13109 calibrator.getInitialBiasYAsAcceleration(by2);
13110 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13111 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13112 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13113 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13114 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13115 final Acceleration bz2 = new Acceleration(0.0,
13116 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13117 calibrator.getInitialBiasZAsAcceleration(bz2);
13118 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13119 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13120 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13121 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13122 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13123 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13124 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13125 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13126 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13127 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13128 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13129 final double[] bias1 = calibrator.getInitialBias();
13130 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
13131 final double[] bias2 = new double[3];
13132 calibrator.getInitialBias(bias2);
13133 assertArrayEquals(bias1, bias2, 0.0);
13134 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13135 assertEquals(b1, ba);
13136 final Matrix b2 = new Matrix(3, 1);
13137 calibrator.getInitialBiasAsMatrix(b2);
13138 assertEquals(b1, b2);
13139 final Matrix ma1 = calibrator.getInitialMa();
13140 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
13141 final Matrix ma2 = new Matrix(3, 3);
13142 calibrator.getInitialMa(ma2);
13143 assertEquals(ma1, ma2);
13144 assertSame(calibrator.getEcefPosition(), ecefPosition);
13145 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13146 final NEDPosition nedPosition1 = new NEDPosition();
13147 assertTrue(calibrator.getNedPosition(nedPosition1));
13148 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13149 assertNull(calibrator.getMeasurements());
13150 assertTrue(calibrator.isCommonAxisUsed());
13151 assertNull(calibrator.getListener());
13152 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13153 assertFalse(calibrator.isReady());
13154 assertFalse(calibrator.isRunning());
13155 assertNull(calibrator.getEstimatedBiases());
13156 assertFalse(calibrator.getEstimatedBiases(null));
13157 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13158 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13159 assertNull(calibrator.getEstimatedBiasFx());
13160 assertNull(calibrator.getEstimatedBiasFy());
13161 assertNull(calibrator.getEstimatedBiasFz());
13162 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13163 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13164 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13165 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13166 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13167 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13168 assertNull(calibrator.getEstimatedMa());
13169 assertNull(calibrator.getEstimatedSx());
13170 assertNull(calibrator.getEstimatedSy());
13171 assertNull(calibrator.getEstimatedSz());
13172 assertNull(calibrator.getEstimatedMxy());
13173 assertNull(calibrator.getEstimatedMxz());
13174 assertNull(calibrator.getEstimatedMyx());
13175 assertNull(calibrator.getEstimatedMyz());
13176 assertNull(calibrator.getEstimatedMzx());
13177 assertNull(calibrator.getEstimatedMzy());
13178 assertNull(calibrator.getEstimatedCovariance());
13179 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13180 }
13181
13182 @Test
13183 public void testConstructor115() throws WrongSizeException {
13184 final Matrix ba = generateBa();
13185 final double biasX = ba.getElementAtIndex(0);
13186 final double biasY = ba.getElementAtIndex(1);
13187 final double biasZ = ba.getElementAtIndex(2);
13188
13189 final Acceleration bx = new Acceleration(biasX,
13190 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13191 final Acceleration by = new Acceleration(biasY,
13192 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13193 final Acceleration bz = new Acceleration(biasZ,
13194 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13195
13196 final Matrix ma = generateMaCommonAxis();
13197 final double sx = ma.getElementAt(0, 0);
13198 final double sy = ma.getElementAt(1, 1);
13199 final double sz = ma.getElementAt(2, 2);
13200
13201 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13202 final double latitude = Math.toRadians(
13203 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13204 final double longitude = Math.toRadians(
13205 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13206 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13207 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13208 final NEDVelocity nedVelocity = new NEDVelocity();
13209 final ECEFPosition ecefPosition = new ECEFPosition();
13210 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13211 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13212 ecefPosition, ecefVelocity);
13213
13214 final KnownPositionAccelerometerCalibrator calibrator =
13215 new KnownPositionAccelerometerCalibrator(ecefPosition,
13216 true, bx, by, bz, sx, sy, sz, this);
13217
13218
13219 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13220 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13221 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13222 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13223 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13224 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13225 final Acceleration bx2 = new Acceleration(0.0,
13226 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13227 calibrator.getInitialBiasXAsAcceleration(bx2);
13228 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13229 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13230 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13231 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13232 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13233 final Acceleration by2 = new Acceleration(0.0,
13234 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13235 calibrator.getInitialBiasYAsAcceleration(by2);
13236 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13237 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13238 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13239 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13240 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13241 final Acceleration bz2 = new Acceleration(0.0,
13242 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13243 calibrator.getInitialBiasZAsAcceleration(bz2);
13244 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13245 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13246 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13247 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13248 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13249 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13250 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13251 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13252 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13253 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13254 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13255 final double[] bias1 = calibrator.getInitialBias();
13256 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
13257 final double[] bias2 = new double[3];
13258 calibrator.getInitialBias(bias2);
13259 assertArrayEquals(bias1, bias2, 0.0);
13260 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13261 assertEquals(b1, ba);
13262 final Matrix b2 = new Matrix(3, 1);
13263 calibrator.getInitialBiasAsMatrix(b2);
13264 assertEquals(b1, b2);
13265 final Matrix ma1 = calibrator.getInitialMa();
13266 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
13267 final Matrix ma2 = new Matrix(3, 3);
13268 calibrator.getInitialMa(ma2);
13269 assertEquals(ma1, ma2);
13270 assertSame(calibrator.getEcefPosition(), ecefPosition);
13271 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13272 final NEDPosition nedPosition1 = new NEDPosition();
13273 assertTrue(calibrator.getNedPosition(nedPosition1));
13274 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13275 assertNull(calibrator.getMeasurements());
13276 assertTrue(calibrator.isCommonAxisUsed());
13277 assertSame(calibrator.getListener(), this);
13278 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13279 assertFalse(calibrator.isReady());
13280 assertFalse(calibrator.isRunning());
13281 assertNull(calibrator.getEstimatedBiases());
13282 assertFalse(calibrator.getEstimatedBiases(null));
13283 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13284 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13285 assertNull(calibrator.getEstimatedBiasFx());
13286 assertNull(calibrator.getEstimatedBiasFy());
13287 assertNull(calibrator.getEstimatedBiasFz());
13288 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13289 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13290 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13291 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13292 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13293 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13294 assertNull(calibrator.getEstimatedMa());
13295 assertNull(calibrator.getEstimatedSx());
13296 assertNull(calibrator.getEstimatedSy());
13297 assertNull(calibrator.getEstimatedSz());
13298 assertNull(calibrator.getEstimatedMxy());
13299 assertNull(calibrator.getEstimatedMxz());
13300 assertNull(calibrator.getEstimatedMyx());
13301 assertNull(calibrator.getEstimatedMyz());
13302 assertNull(calibrator.getEstimatedMzx());
13303 assertNull(calibrator.getEstimatedMzy());
13304 assertNull(calibrator.getEstimatedCovariance());
13305 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13306 }
13307
13308 @Test
13309 public void testConstructor116() throws WrongSizeException {
13310 final Collection<StandardDeviationBodyKinematics> measurements =
13311 Collections.emptyList();
13312
13313 final Matrix ba = generateBa();
13314 final double biasX = ba.getElementAtIndex(0);
13315 final double biasY = ba.getElementAtIndex(1);
13316 final double biasZ = ba.getElementAtIndex(2);
13317
13318 final Acceleration bx = new Acceleration(biasX,
13319 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13320 final Acceleration by = new Acceleration(biasY,
13321 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13322 final Acceleration bz = new Acceleration(biasZ,
13323 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13324
13325 final Matrix ma = generateMaCommonAxis();
13326 final double sx = ma.getElementAt(0, 0);
13327 final double sy = ma.getElementAt(1, 1);
13328 final double sz = ma.getElementAt(2, 2);
13329
13330 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13331 final double latitude = Math.toRadians(
13332 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13333 final double longitude = Math.toRadians(
13334 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13335 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13336 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13337 final NEDVelocity nedVelocity = new NEDVelocity();
13338 final ECEFPosition ecefPosition = new ECEFPosition();
13339 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13340 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13341 ecefPosition, ecefVelocity);
13342
13343 final KnownPositionAccelerometerCalibrator calibrator =
13344 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
13345 true, bx, by, bz, sx, sy, sz);
13346
13347
13348 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13349 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13350 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13351 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13352 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13353 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13354 final Acceleration bx2 = new Acceleration(0.0,
13355 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13356 calibrator.getInitialBiasXAsAcceleration(bx2);
13357 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13358 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13359 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13360 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13361 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13362 final Acceleration by2 = new Acceleration(0.0,
13363 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13364 calibrator.getInitialBiasYAsAcceleration(by2);
13365 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13366 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13367 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13368 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13369 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13370 final Acceleration bz2 = new Acceleration(0.0,
13371 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13372 calibrator.getInitialBiasZAsAcceleration(bz2);
13373 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13374 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13375 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13376 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13377 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13378 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13379 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13380 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13381 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13382 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13383 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13384 final double[] bias1 = calibrator.getInitialBias();
13385 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
13386 final double[] bias2 = new double[3];
13387 calibrator.getInitialBias(bias2);
13388 assertArrayEquals(bias1, bias2, 0.0);
13389 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13390 assertEquals(b1, ba);
13391 final Matrix b2 = new Matrix(3, 1);
13392 calibrator.getInitialBiasAsMatrix(b2);
13393 assertEquals(b1, b2);
13394 final Matrix ma1 = calibrator.getInitialMa();
13395 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
13396 final Matrix ma2 = new Matrix(3, 3);
13397 calibrator.getInitialMa(ma2);
13398 assertEquals(ma1, ma2);
13399 assertSame(calibrator.getEcefPosition(), ecefPosition);
13400 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13401 final NEDPosition nedPosition1 = new NEDPosition();
13402 assertTrue(calibrator.getNedPosition(nedPosition1));
13403 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13404 assertSame(calibrator.getMeasurements(), measurements);
13405 assertTrue(calibrator.isCommonAxisUsed());
13406 assertNull(calibrator.getListener());
13407 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13408 assertFalse(calibrator.isReady());
13409 assertFalse(calibrator.isRunning());
13410 assertNull(calibrator.getEstimatedBiases());
13411 assertFalse(calibrator.getEstimatedBiases(null));
13412 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13413 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13414 assertNull(calibrator.getEstimatedBiasFx());
13415 assertNull(calibrator.getEstimatedBiasFy());
13416 assertNull(calibrator.getEstimatedBiasFz());
13417 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13418 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13419 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13420 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13421 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13422 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13423 assertNull(calibrator.getEstimatedMa());
13424 assertNull(calibrator.getEstimatedSx());
13425 assertNull(calibrator.getEstimatedSy());
13426 assertNull(calibrator.getEstimatedSz());
13427 assertNull(calibrator.getEstimatedMxy());
13428 assertNull(calibrator.getEstimatedMxz());
13429 assertNull(calibrator.getEstimatedMyx());
13430 assertNull(calibrator.getEstimatedMyz());
13431 assertNull(calibrator.getEstimatedMzx());
13432 assertNull(calibrator.getEstimatedMzy());
13433 assertNull(calibrator.getEstimatedCovariance());
13434 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13435 }
13436
13437 @Test
13438 public void testConstructor117() throws WrongSizeException {
13439 final Collection<StandardDeviationBodyKinematics> measurements =
13440 Collections.emptyList();
13441
13442 final Matrix ba = generateBa();
13443 final double biasX = ba.getElementAtIndex(0);
13444 final double biasY = ba.getElementAtIndex(1);
13445 final double biasZ = ba.getElementAtIndex(2);
13446
13447 final Acceleration bx = new Acceleration(biasX,
13448 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13449 final Acceleration by = new Acceleration(biasY,
13450 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13451 final Acceleration bz = new Acceleration(biasZ,
13452 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13453
13454 final Matrix ma = generateMaCommonAxis();
13455 final double sx = ma.getElementAt(0, 0);
13456 final double sy = ma.getElementAt(1, 1);
13457 final double sz = ma.getElementAt(2, 2);
13458
13459 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13460 final double latitude = Math.toRadians(
13461 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13462 final double longitude = Math.toRadians(
13463 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13464 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13465 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13466 final NEDVelocity nedVelocity = new NEDVelocity();
13467 final ECEFPosition ecefPosition = new ECEFPosition();
13468 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13469 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13470 ecefPosition, ecefVelocity);
13471
13472 final KnownPositionAccelerometerCalibrator calibrator =
13473 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
13474 true, bx, by, bz, sx, sy, sz, this);
13475
13476
13477 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13478 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13479 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13480 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13481 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13482 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13483 final Acceleration bx2 = new Acceleration(0.0,
13484 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13485 calibrator.getInitialBiasXAsAcceleration(bx2);
13486 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13487 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13488 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13489 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13490 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13491 final Acceleration by2 = new Acceleration(0.0,
13492 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13493 calibrator.getInitialBiasYAsAcceleration(by2);
13494 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13495 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13496 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13497 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13498 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13499 final Acceleration bz2 = new Acceleration(0.0,
13500 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13501 calibrator.getInitialBiasZAsAcceleration(bz2);
13502 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13503 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13504 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13505 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13506 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13507 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13508 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13509 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13510 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13511 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13512 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13513 final double[] bias1 = calibrator.getInitialBias();
13514 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
13515 final double[] bias2 = new double[3];
13516 calibrator.getInitialBias(bias2);
13517 assertArrayEquals(bias1, bias2, 0.0);
13518 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13519 assertEquals(b1, ba);
13520 final Matrix b2 = new Matrix(3, 1);
13521 calibrator.getInitialBiasAsMatrix(b2);
13522 assertEquals(b1, b2);
13523 final Matrix ma1 = calibrator.getInitialMa();
13524 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
13525 final Matrix ma2 = new Matrix(3, 3);
13526 calibrator.getInitialMa(ma2);
13527 assertEquals(ma1, ma2);
13528 assertSame(calibrator.getEcefPosition(), ecefPosition);
13529 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13530 final NEDPosition nedPosition1 = new NEDPosition();
13531 assertTrue(calibrator.getNedPosition(nedPosition1));
13532 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13533 assertSame(calibrator.getMeasurements(), measurements);
13534 assertTrue(calibrator.isCommonAxisUsed());
13535 assertSame(calibrator.getListener(), this);
13536 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13537 assertFalse(calibrator.isReady());
13538 assertFalse(calibrator.isRunning());
13539 assertNull(calibrator.getEstimatedBiases());
13540 assertFalse(calibrator.getEstimatedBiases(null));
13541 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13542 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13543 assertNull(calibrator.getEstimatedBiasFx());
13544 assertNull(calibrator.getEstimatedBiasFy());
13545 assertNull(calibrator.getEstimatedBiasFz());
13546 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13547 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13548 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13549 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13550 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13551 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13552 assertNull(calibrator.getEstimatedMa());
13553 assertNull(calibrator.getEstimatedSx());
13554 assertNull(calibrator.getEstimatedSy());
13555 assertNull(calibrator.getEstimatedSz());
13556 assertNull(calibrator.getEstimatedMxy());
13557 assertNull(calibrator.getEstimatedMxz());
13558 assertNull(calibrator.getEstimatedMyx());
13559 assertNull(calibrator.getEstimatedMyz());
13560 assertNull(calibrator.getEstimatedMzx());
13561 assertNull(calibrator.getEstimatedMzy());
13562 assertNull(calibrator.getEstimatedCovariance());
13563 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13564 }
13565
13566 @Test
13567 public void testConstructor118() throws WrongSizeException {
13568 final Matrix ba = generateBa();
13569 final double biasX = ba.getElementAtIndex(0);
13570 final double biasY = ba.getElementAtIndex(1);
13571 final double biasZ = ba.getElementAtIndex(2);
13572
13573 final Matrix ma = generateMaCommonAxis();
13574 final double sx = ma.getElementAt(0, 0);
13575 final double sy = ma.getElementAt(1, 1);
13576 final double sz = ma.getElementAt(2, 2);
13577 final double mxy = ma.getElementAt(0, 1);
13578 final double mxz = ma.getElementAt(0, 2);
13579 final double myx = ma.getElementAt(1, 0);
13580 final double myz = ma.getElementAt(1, 2);
13581 final double mzx = ma.getElementAt(2, 0);
13582 final double mzy = ma.getElementAt(2, 1);
13583
13584 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13585 final double latitude = Math.toRadians(
13586 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13587 final double longitude = Math.toRadians(
13588 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13589 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13590 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13591 final NEDVelocity nedVelocity = new NEDVelocity();
13592 final ECEFPosition ecefPosition = new ECEFPosition();
13593 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13594 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13595 ecefPosition, ecefVelocity);
13596
13597 final KnownPositionAccelerometerCalibrator calibrator =
13598 new KnownPositionAccelerometerCalibrator(ecefPosition,
13599 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
13600 myx, myz, mzx, mzy);
13601
13602
13603 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13604 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13605 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13606 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13607 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13608 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13609 final Acceleration bx2 = new Acceleration(0.0,
13610 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13611 calibrator.getInitialBiasXAsAcceleration(bx2);
13612 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13613 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13614 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13615 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13616 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13617 final Acceleration by2 = new Acceleration(0.0,
13618 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13619 calibrator.getInitialBiasYAsAcceleration(by2);
13620 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13621 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13622 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13623 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13624 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13625 final Acceleration bz2 = new Acceleration(0.0,
13626 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13627 calibrator.getInitialBiasZAsAcceleration(bz2);
13628 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13629 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13630 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13631 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13632 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13633 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
13634 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
13635 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
13636 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
13637 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
13638 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
13639 final double[] bias1 = calibrator.getInitialBias();
13640 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
13641 final double[] bias2 = new double[3];
13642 calibrator.getInitialBias(bias2);
13643 assertArrayEquals(bias1, bias2, 0.0);
13644 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13645 assertEquals(b1, ba);
13646 final Matrix b2 = new Matrix(3, 1);
13647 calibrator.getInitialBiasAsMatrix(b2);
13648 assertEquals(b1, b2);
13649 final Matrix ma1 = new Matrix(3, 3);
13650 ma1.setSubmatrix(0, 0,
13651 2, 2,
13652 new double[]{ sx, myx, mzx,
13653 mxy, sy, mzy,
13654 mxz, myz, sz});
13655 assertEquals(calibrator.getInitialMa(), ma1);
13656 final Matrix ma2 = new Matrix(3, 3);
13657 calibrator.getInitialMa(ma2);
13658 assertEquals(ma1, ma2);
13659 assertSame(calibrator.getEcefPosition(), ecefPosition);
13660 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13661 final NEDPosition nedPosition1 = new NEDPosition();
13662 assertTrue(calibrator.getNedPosition(nedPosition1));
13663 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13664 assertNull(calibrator.getMeasurements());
13665 assertFalse(calibrator.isCommonAxisUsed());
13666 assertNull(calibrator.getListener());
13667 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13668 assertFalse(calibrator.isReady());
13669 assertFalse(calibrator.isRunning());
13670 assertNull(calibrator.getEstimatedBiases());
13671 assertFalse(calibrator.getEstimatedBiases(null));
13672 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13673 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13674 assertNull(calibrator.getEstimatedBiasFx());
13675 assertNull(calibrator.getEstimatedBiasFy());
13676 assertNull(calibrator.getEstimatedBiasFz());
13677 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13678 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13679 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13680 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13681 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13682 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13683 assertNull(calibrator.getEstimatedMa());
13684 assertNull(calibrator.getEstimatedSx());
13685 assertNull(calibrator.getEstimatedSy());
13686 assertNull(calibrator.getEstimatedSz());
13687 assertNull(calibrator.getEstimatedMxy());
13688 assertNull(calibrator.getEstimatedMxz());
13689 assertNull(calibrator.getEstimatedMyx());
13690 assertNull(calibrator.getEstimatedMyz());
13691 assertNull(calibrator.getEstimatedMzx());
13692 assertNull(calibrator.getEstimatedMzy());
13693 assertNull(calibrator.getEstimatedCovariance());
13694 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13695 }
13696
13697 @Test
13698 public void testConstructor119() throws WrongSizeException {
13699 final Collection<StandardDeviationBodyKinematics> measurements =
13700 Collections.emptyList();
13701
13702 final Matrix ba = generateBa();
13703 final double biasX = ba.getElementAtIndex(0);
13704 final double biasY = ba.getElementAtIndex(1);
13705 final double biasZ = ba.getElementAtIndex(2);
13706
13707 final Matrix ma = generateMaCommonAxis();
13708 final double sx = ma.getElementAt(0, 0);
13709 final double sy = ma.getElementAt(1, 1);
13710 final double sz = ma.getElementAt(2, 2);
13711 final double mxy = ma.getElementAt(0, 1);
13712 final double mxz = ma.getElementAt(0, 2);
13713 final double myx = ma.getElementAt(1, 0);
13714 final double myz = ma.getElementAt(1, 2);
13715 final double mzx = ma.getElementAt(2, 0);
13716 final double mzy = ma.getElementAt(2, 1);
13717
13718 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13719 final double latitude = Math.toRadians(
13720 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13721 final double longitude = Math.toRadians(
13722 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13723 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13724 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13725 final NEDVelocity nedVelocity = new NEDVelocity();
13726 final ECEFPosition ecefPosition = new ECEFPosition();
13727 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13728 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13729 ecefPosition, ecefVelocity);
13730
13731 final KnownPositionAccelerometerCalibrator calibrator =
13732 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
13733 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
13734 myx, myz, mzx, mzy);
13735
13736
13737 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13738 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13739 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13740 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13741 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13742 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13743 final Acceleration bx2 = new Acceleration(0.0,
13744 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13745 calibrator.getInitialBiasXAsAcceleration(bx2);
13746 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13747 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13748 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13749 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13750 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13751 final Acceleration by2 = new Acceleration(0.0,
13752 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13753 calibrator.getInitialBiasYAsAcceleration(by2);
13754 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13755 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13756 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13757 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13758 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13759 final Acceleration bz2 = new Acceleration(0.0,
13760 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13761 calibrator.getInitialBiasZAsAcceleration(bz2);
13762 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13763 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13764 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13765 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13766 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13767 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
13768 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
13769 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
13770 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
13771 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
13772 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
13773 final double[] bias1 = calibrator.getInitialBias();
13774 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
13775 final double[] bias2 = new double[3];
13776 calibrator.getInitialBias(bias2);
13777 assertArrayEquals(bias1, bias2, 0.0);
13778 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13779 assertEquals(b1, ba);
13780 final Matrix b2 = new Matrix(3, 1);
13781 calibrator.getInitialBiasAsMatrix(b2);
13782 assertEquals(b1, b2);
13783 final Matrix ma1 = new Matrix(3, 3);
13784 ma1.setSubmatrix(0, 0,
13785 2, 2,
13786 new double[]{ sx, myx, mzx,
13787 mxy, sy, mzy,
13788 mxz, myz, sz});
13789 assertEquals(calibrator.getInitialMa(), ma1);
13790 final Matrix ma2 = new Matrix(3, 3);
13791 calibrator.getInitialMa(ma2);
13792 assertEquals(ma1, ma2);
13793 assertSame(calibrator.getEcefPosition(), ecefPosition);
13794 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13795 final NEDPosition nedPosition1 = new NEDPosition();
13796 assertTrue(calibrator.getNedPosition(nedPosition1));
13797 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13798 assertSame(calibrator.getMeasurements(), measurements);
13799 assertFalse(calibrator.isCommonAxisUsed());
13800 assertNull(calibrator.getListener());
13801 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13802 assertFalse(calibrator.isReady());
13803 assertFalse(calibrator.isRunning());
13804 assertNull(calibrator.getEstimatedBiases());
13805 assertFalse(calibrator.getEstimatedBiases(null));
13806 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13807 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13808 assertNull(calibrator.getEstimatedBiasFx());
13809 assertNull(calibrator.getEstimatedBiasFy());
13810 assertNull(calibrator.getEstimatedBiasFz());
13811 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13812 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13813 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13814 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13815 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13816 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13817 assertNull(calibrator.getEstimatedMa());
13818 assertNull(calibrator.getEstimatedSx());
13819 assertNull(calibrator.getEstimatedSy());
13820 assertNull(calibrator.getEstimatedSz());
13821 assertNull(calibrator.getEstimatedMxy());
13822 assertNull(calibrator.getEstimatedMxz());
13823 assertNull(calibrator.getEstimatedMyx());
13824 assertNull(calibrator.getEstimatedMyz());
13825 assertNull(calibrator.getEstimatedMzx());
13826 assertNull(calibrator.getEstimatedMzy());
13827 assertNull(calibrator.getEstimatedCovariance());
13828 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13829 }
13830
13831 @Test
13832 public void testConstructor120() throws WrongSizeException {
13833 final Collection<StandardDeviationBodyKinematics> measurements =
13834 Collections.emptyList();
13835
13836 final Matrix ba = generateBa();
13837 final double biasX = ba.getElementAtIndex(0);
13838 final double biasY = ba.getElementAtIndex(1);
13839 final double biasZ = ba.getElementAtIndex(2);
13840
13841 final Matrix ma = generateMaCommonAxis();
13842 final double sx = ma.getElementAt(0, 0);
13843 final double sy = ma.getElementAt(1, 1);
13844 final double sz = ma.getElementAt(2, 2);
13845 final double mxy = ma.getElementAt(0, 1);
13846 final double mxz = ma.getElementAt(0, 2);
13847 final double myx = ma.getElementAt(1, 0);
13848 final double myz = ma.getElementAt(1, 2);
13849 final double mzx = ma.getElementAt(2, 0);
13850 final double mzy = ma.getElementAt(2, 1);
13851
13852 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13853 final double latitude = Math.toRadians(
13854 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13855 final double longitude = Math.toRadians(
13856 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13857 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13858 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13859 final NEDVelocity nedVelocity = new NEDVelocity();
13860 final ECEFPosition ecefPosition = new ECEFPosition();
13861 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13862 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13863 ecefPosition, ecefVelocity);
13864
13865 final KnownPositionAccelerometerCalibrator calibrator =
13866 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
13867 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
13868 myx, myz, mzx, mzy, this);
13869
13870
13871 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13872 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13873 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13874 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13875 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13876 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13877 final Acceleration bx2 = new Acceleration(0.0,
13878 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13879 calibrator.getInitialBiasXAsAcceleration(bx2);
13880 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13881 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13882 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13883 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13884 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13885 final Acceleration by2 = new Acceleration(0.0,
13886 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13887 calibrator.getInitialBiasYAsAcceleration(by2);
13888 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13889 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13890 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13891 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13892 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13893 final Acceleration bz2 = new Acceleration(0.0,
13894 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13895 calibrator.getInitialBiasZAsAcceleration(bz2);
13896 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13897 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13898 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13899 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13900 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13901 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
13902 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
13903 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
13904 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
13905 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
13906 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
13907 final double[] bias1 = calibrator.getInitialBias();
13908 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
13909 final double[] bias2 = new double[3];
13910 calibrator.getInitialBias(bias2);
13911 assertArrayEquals(bias1, bias2, 0.0);
13912 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13913 assertEquals(b1, ba);
13914 final Matrix b2 = new Matrix(3, 1);
13915 calibrator.getInitialBiasAsMatrix(b2);
13916 assertEquals(b1, b2);
13917 final Matrix ma1 = new Matrix(3, 3);
13918 ma1.setSubmatrix(0, 0,
13919 2, 2,
13920 new double[]{ sx, myx, mzx,
13921 mxy, sy, mzy,
13922 mxz, myz, sz});
13923 assertEquals(calibrator.getInitialMa(), ma1);
13924 final Matrix ma2 = new Matrix(3, 3);
13925 calibrator.getInitialMa(ma2);
13926 assertEquals(ma1, ma2);
13927 assertSame(calibrator.getEcefPosition(), ecefPosition);
13928 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13929 final NEDPosition nedPosition1 = new NEDPosition();
13930 assertTrue(calibrator.getNedPosition(nedPosition1));
13931 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13932 assertSame(calibrator.getMeasurements(), measurements);
13933 assertFalse(calibrator.isCommonAxisUsed());
13934 assertSame(calibrator.getListener(), this);
13935 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13936 assertFalse(calibrator.isReady());
13937 assertFalse(calibrator.isRunning());
13938 assertNull(calibrator.getEstimatedBiases());
13939 assertFalse(calibrator.getEstimatedBiases(null));
13940 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13941 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13942 assertNull(calibrator.getEstimatedBiasFx());
13943 assertNull(calibrator.getEstimatedBiasFy());
13944 assertNull(calibrator.getEstimatedBiasFz());
13945 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13946 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13947 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13948 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13949 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13950 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13951 assertNull(calibrator.getEstimatedMa());
13952 assertNull(calibrator.getEstimatedSx());
13953 assertNull(calibrator.getEstimatedSy());
13954 assertNull(calibrator.getEstimatedSz());
13955 assertNull(calibrator.getEstimatedMxy());
13956 assertNull(calibrator.getEstimatedMxz());
13957 assertNull(calibrator.getEstimatedMyx());
13958 assertNull(calibrator.getEstimatedMyz());
13959 assertNull(calibrator.getEstimatedMzx());
13960 assertNull(calibrator.getEstimatedMzy());
13961 assertNull(calibrator.getEstimatedCovariance());
13962 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13963 }
13964
13965 @Test
13966 public void testConstructor121() throws WrongSizeException {
13967 final Matrix ba = generateBa();
13968 final double biasX = ba.getElementAtIndex(0);
13969 final double biasY = ba.getElementAtIndex(1);
13970 final double biasZ = ba.getElementAtIndex(2);
13971
13972 final Matrix ma = generateMaCommonAxis();
13973 final double sx = ma.getElementAt(0, 0);
13974 final double sy = ma.getElementAt(1, 1);
13975 final double sz = ma.getElementAt(2, 2);
13976 final double mxy = ma.getElementAt(0, 1);
13977 final double mxz = ma.getElementAt(0, 2);
13978 final double myx = ma.getElementAt(1, 0);
13979 final double myz = ma.getElementAt(1, 2);
13980 final double mzx = ma.getElementAt(2, 0);
13981 final double mzy = ma.getElementAt(2, 1);
13982
13983 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13984 final double latitude = Math.toRadians(
13985 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13986 final double longitude = Math.toRadians(
13987 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13988 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13989 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13990 final NEDVelocity nedVelocity = new NEDVelocity();
13991 final ECEFPosition ecefPosition = new ECEFPosition();
13992 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13993 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13994 ecefPosition, ecefVelocity);
13995
13996 final KnownPositionAccelerometerCalibrator calibrator =
13997 new KnownPositionAccelerometerCalibrator(ecefPosition,
13998 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
13999 myx, myz, mzx, mzy);
14000
14001
14002 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14003 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14004 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14005 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14006 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14007 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14008 final Acceleration bx2 = new Acceleration(0.0,
14009 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14010 calibrator.getInitialBiasXAsAcceleration(bx2);
14011 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14012 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14013 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14014 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14015 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14016 final Acceleration by2 = new Acceleration(0.0,
14017 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14018 calibrator.getInitialBiasYAsAcceleration(by2);
14019 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14020 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14021 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14022 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14023 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14024 final Acceleration bz2 = new Acceleration(0.0,
14025 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14026 calibrator.getInitialBiasZAsAcceleration(bz2);
14027 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14028 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14029 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14030 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14031 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14032 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14033 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14034 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14035 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14036 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14037 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14038 final double[] bias1 = calibrator.getInitialBias();
14039 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
14040 final double[] bias2 = new double[3];
14041 calibrator.getInitialBias(bias2);
14042 assertArrayEquals(bias1, bias2, 0.0);
14043 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14044 assertEquals(b1, ba);
14045 final Matrix b2 = new Matrix(3, 1);
14046 calibrator.getInitialBiasAsMatrix(b2);
14047 assertEquals(b1, b2);
14048 final Matrix ma1 = new Matrix(3, 3);
14049 ma1.setSubmatrix(0, 0,
14050 2, 2,
14051 new double[]{ sx, myx, mzx,
14052 mxy, sy, mzy,
14053 mxz, myz, sz});
14054 assertEquals(calibrator.getInitialMa(), ma1);
14055 final Matrix ma2 = new Matrix(3, 3);
14056 calibrator.getInitialMa(ma2);
14057 assertEquals(ma1, ma2);
14058 assertSame(calibrator.getEcefPosition(), ecefPosition);
14059 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14060 final NEDPosition nedPosition1 = new NEDPosition();
14061 assertTrue(calibrator.getNedPosition(nedPosition1));
14062 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14063 assertNull(calibrator.getMeasurements());
14064 assertTrue(calibrator.isCommonAxisUsed());
14065 assertNull(calibrator.getListener());
14066 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14067 assertFalse(calibrator.isReady());
14068 assertFalse(calibrator.isRunning());
14069 assertNull(calibrator.getEstimatedBiases());
14070 assertFalse(calibrator.getEstimatedBiases(null));
14071 assertNull(calibrator.getEstimatedBiasesAsMatrix());
14072 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14073 assertNull(calibrator.getEstimatedBiasFx());
14074 assertNull(calibrator.getEstimatedBiasFy());
14075 assertNull(calibrator.getEstimatedBiasFz());
14076 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14077 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14078 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14079 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14080 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14081 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14082 assertNull(calibrator.getEstimatedMa());
14083 assertNull(calibrator.getEstimatedSx());
14084 assertNull(calibrator.getEstimatedSy());
14085 assertNull(calibrator.getEstimatedSz());
14086 assertNull(calibrator.getEstimatedMxy());
14087 assertNull(calibrator.getEstimatedMxz());
14088 assertNull(calibrator.getEstimatedMyx());
14089 assertNull(calibrator.getEstimatedMyz());
14090 assertNull(calibrator.getEstimatedMzx());
14091 assertNull(calibrator.getEstimatedMzy());
14092 assertNull(calibrator.getEstimatedCovariance());
14093 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14094 }
14095
14096 @Test
14097 public void testConstructor122() throws WrongSizeException {
14098 final Matrix ba = generateBa();
14099 final double biasX = ba.getElementAtIndex(0);
14100 final double biasY = ba.getElementAtIndex(1);
14101 final double biasZ = ba.getElementAtIndex(2);
14102
14103 final Matrix ma = generateMaCommonAxis();
14104 final double sx = ma.getElementAt(0, 0);
14105 final double sy = ma.getElementAt(1, 1);
14106 final double sz = ma.getElementAt(2, 2);
14107 final double mxy = ma.getElementAt(0, 1);
14108 final double mxz = ma.getElementAt(0, 2);
14109 final double myx = ma.getElementAt(1, 0);
14110 final double myz = ma.getElementAt(1, 2);
14111 final double mzx = ma.getElementAt(2, 0);
14112 final double mzy = ma.getElementAt(2, 1);
14113
14114 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14115 final double latitude = Math.toRadians(
14116 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14117 final double longitude = Math.toRadians(
14118 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14119 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14120 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14121 final NEDVelocity nedVelocity = new NEDVelocity();
14122 final ECEFPosition ecefPosition = new ECEFPosition();
14123 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14124 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14125 ecefPosition, ecefVelocity);
14126
14127 final KnownPositionAccelerometerCalibrator calibrator =
14128 new KnownPositionAccelerometerCalibrator(ecefPosition,
14129 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14130 myx, myz, mzx, mzy, this);
14131
14132
14133 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14134 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14135 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14136 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14137 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14138 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14139 final Acceleration bx2 = new Acceleration(0.0,
14140 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14141 calibrator.getInitialBiasXAsAcceleration(bx2);
14142 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14143 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14144 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14145 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14146 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14147 final Acceleration by2 = new Acceleration(0.0,
14148 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14149 calibrator.getInitialBiasYAsAcceleration(by2);
14150 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14151 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14152 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14153 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14154 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14155 final Acceleration bz2 = new Acceleration(0.0,
14156 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14157 calibrator.getInitialBiasZAsAcceleration(bz2);
14158 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14159 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14160 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14161 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14162 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14163 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14164 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14165 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14166 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14167 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14168 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14169 final double[] bias1 = calibrator.getInitialBias();
14170 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
14171 final double[] bias2 = new double[3];
14172 calibrator.getInitialBias(bias2);
14173 assertArrayEquals(bias1, bias2, 0.0);
14174 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14175 assertEquals(b1, ba);
14176 final Matrix b2 = new Matrix(3, 1);
14177 calibrator.getInitialBiasAsMatrix(b2);
14178 assertEquals(b1, b2);
14179 final Matrix ma1 = new Matrix(3, 3);
14180 ma1.setSubmatrix(0, 0,
14181 2, 2,
14182 new double[]{ sx, myx, mzx,
14183 mxy, sy, mzy,
14184 mxz, myz, sz});
14185 assertEquals(calibrator.getInitialMa(), ma1);
14186 final Matrix ma2 = new Matrix(3, 3);
14187 calibrator.getInitialMa(ma2);
14188 assertEquals(ma1, ma2);
14189 assertSame(calibrator.getEcefPosition(), ecefPosition);
14190 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14191 final NEDPosition nedPosition1 = new NEDPosition();
14192 assertTrue(calibrator.getNedPosition(nedPosition1));
14193 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14194 assertNull(calibrator.getMeasurements());
14195 assertTrue(calibrator.isCommonAxisUsed());
14196 assertSame(calibrator.getListener(), this);
14197 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14198 assertFalse(calibrator.isReady());
14199 assertFalse(calibrator.isRunning());
14200 assertNull(calibrator.getEstimatedBiases());
14201 assertFalse(calibrator.getEstimatedBiases(null));
14202 assertNull(calibrator.getEstimatedBiasesAsMatrix());
14203 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14204 assertNull(calibrator.getEstimatedBiasFx());
14205 assertNull(calibrator.getEstimatedBiasFy());
14206 assertNull(calibrator.getEstimatedBiasFz());
14207 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14208 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14209 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14210 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14211 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14212 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14213 assertNull(calibrator.getEstimatedMa());
14214 assertNull(calibrator.getEstimatedSx());
14215 assertNull(calibrator.getEstimatedSy());
14216 assertNull(calibrator.getEstimatedSz());
14217 assertNull(calibrator.getEstimatedMxy());
14218 assertNull(calibrator.getEstimatedMxz());
14219 assertNull(calibrator.getEstimatedMyx());
14220 assertNull(calibrator.getEstimatedMyz());
14221 assertNull(calibrator.getEstimatedMzx());
14222 assertNull(calibrator.getEstimatedMzy());
14223 assertNull(calibrator.getEstimatedCovariance());
14224 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14225 }
14226
14227 @Test
14228 public void testConstructor123() throws WrongSizeException {
14229 final Collection<StandardDeviationBodyKinematics> measurements =
14230 Collections.emptyList();
14231
14232 final Matrix ba = generateBa();
14233 final double biasX = ba.getElementAtIndex(0);
14234 final double biasY = ba.getElementAtIndex(1);
14235 final double biasZ = ba.getElementAtIndex(2);
14236
14237 final Matrix ma = generateMaCommonAxis();
14238 final double sx = ma.getElementAt(0, 0);
14239 final double sy = ma.getElementAt(1, 1);
14240 final double sz = ma.getElementAt(2, 2);
14241 final double mxy = ma.getElementAt(0, 1);
14242 final double mxz = ma.getElementAt(0, 2);
14243 final double myx = ma.getElementAt(1, 0);
14244 final double myz = ma.getElementAt(1, 2);
14245 final double mzx = ma.getElementAt(2, 0);
14246 final double mzy = ma.getElementAt(2, 1);
14247
14248 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14249 final double latitude = Math.toRadians(
14250 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14251 final double longitude = Math.toRadians(
14252 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14253 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14254 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14255 final NEDVelocity nedVelocity = new NEDVelocity();
14256 final ECEFPosition ecefPosition = new ECEFPosition();
14257 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14258 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14259 ecefPosition, ecefVelocity);
14260
14261 final KnownPositionAccelerometerCalibrator calibrator =
14262 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
14263 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14264 myx, myz, mzx, mzy);
14265
14266
14267 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14268 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14269 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14270 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14271 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14272 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14273 final Acceleration bx2 = new Acceleration(0.0,
14274 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14275 calibrator.getInitialBiasXAsAcceleration(bx2);
14276 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14277 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14278 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14279 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14280 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14281 final Acceleration by2 = new Acceleration(0.0,
14282 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14283 calibrator.getInitialBiasYAsAcceleration(by2);
14284 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14285 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14286 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14287 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14288 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14289 final Acceleration bz2 = new Acceleration(0.0,
14290 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14291 calibrator.getInitialBiasZAsAcceleration(bz2);
14292 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14293 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14294 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14295 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14296 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14297 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14298 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14299 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14300 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14301 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14302 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14303 final double[] bias1 = calibrator.getInitialBias();
14304 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
14305 final double[] bias2 = new double[3];
14306 calibrator.getInitialBias(bias2);
14307 assertArrayEquals(bias1, bias2, 0.0);
14308 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14309 assertEquals(b1, ba);
14310 final Matrix b2 = new Matrix(3, 1);
14311 calibrator.getInitialBiasAsMatrix(b2);
14312 assertEquals(b1, b2);
14313 final Matrix ma1 = new Matrix(3, 3);
14314 ma1.setSubmatrix(0, 0,
14315 2, 2,
14316 new double[]{ sx, myx, mzx,
14317 mxy, sy, mzy,
14318 mxz, myz, sz});
14319 assertEquals(calibrator.getInitialMa(), ma1);
14320 final Matrix ma2 = new Matrix(3, 3);
14321 calibrator.getInitialMa(ma2);
14322 assertEquals(ma1, ma2);
14323 assertSame(calibrator.getEcefPosition(), ecefPosition);
14324 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14325 final NEDPosition nedPosition1 = new NEDPosition();
14326 assertTrue(calibrator.getNedPosition(nedPosition1));
14327 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14328 assertSame(calibrator.getMeasurements(), measurements);
14329 assertTrue(calibrator.isCommonAxisUsed());
14330 assertNull(calibrator.getListener());
14331 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14332 assertFalse(calibrator.isReady());
14333 assertFalse(calibrator.isRunning());
14334 assertNull(calibrator.getEstimatedBiases());
14335 assertFalse(calibrator.getEstimatedBiases(null));
14336 assertNull(calibrator.getEstimatedBiasesAsMatrix());
14337 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14338 assertNull(calibrator.getEstimatedBiasFx());
14339 assertNull(calibrator.getEstimatedBiasFy());
14340 assertNull(calibrator.getEstimatedBiasFz());
14341 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14342 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14343 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14344 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14345 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14346 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14347 assertNull(calibrator.getEstimatedMa());
14348 assertNull(calibrator.getEstimatedSx());
14349 assertNull(calibrator.getEstimatedSy());
14350 assertNull(calibrator.getEstimatedSz());
14351 assertNull(calibrator.getEstimatedMxy());
14352 assertNull(calibrator.getEstimatedMxz());
14353 assertNull(calibrator.getEstimatedMyx());
14354 assertNull(calibrator.getEstimatedMyz());
14355 assertNull(calibrator.getEstimatedMzx());
14356 assertNull(calibrator.getEstimatedMzy());
14357 assertNull(calibrator.getEstimatedCovariance());
14358 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14359 }
14360
14361 @Test
14362 public void testConstructor124() throws WrongSizeException {
14363 final Collection<StandardDeviationBodyKinematics> measurements =
14364 Collections.emptyList();
14365
14366 final Matrix ba = generateBa();
14367 final double biasX = ba.getElementAtIndex(0);
14368 final double biasY = ba.getElementAtIndex(1);
14369 final double biasZ = ba.getElementAtIndex(2);
14370
14371 final Matrix ma = generateMaCommonAxis();
14372 final double sx = ma.getElementAt(0, 0);
14373 final double sy = ma.getElementAt(1, 1);
14374 final double sz = ma.getElementAt(2, 2);
14375 final double mxy = ma.getElementAt(0, 1);
14376 final double mxz = ma.getElementAt(0, 2);
14377 final double myx = ma.getElementAt(1, 0);
14378 final double myz = ma.getElementAt(1, 2);
14379 final double mzx = ma.getElementAt(2, 0);
14380 final double mzy = ma.getElementAt(2, 1);
14381
14382 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14383 final double latitude = Math.toRadians(
14384 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14385 final double longitude = Math.toRadians(
14386 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14387 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14388 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14389 final NEDVelocity nedVelocity = new NEDVelocity();
14390 final ECEFPosition ecefPosition = new ECEFPosition();
14391 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14392 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14393 ecefPosition, ecefVelocity);
14394
14395 final KnownPositionAccelerometerCalibrator calibrator =
14396 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
14397 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14398 myx, myz, mzx, mzy, this);
14399
14400
14401 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14402 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14403 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14404 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14405 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14406 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14407 final Acceleration bx2 = new Acceleration(0.0,
14408 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14409 calibrator.getInitialBiasXAsAcceleration(bx2);
14410 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14411 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14412 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14413 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14414 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14415 final Acceleration by2 = new Acceleration(0.0,
14416 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14417 calibrator.getInitialBiasYAsAcceleration(by2);
14418 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14419 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14420 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14421 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14422 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14423 final Acceleration bz2 = new Acceleration(0.0,
14424 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14425 calibrator.getInitialBiasZAsAcceleration(bz2);
14426 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14427 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14428 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14429 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14430 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14431 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14432 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14433 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14434 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14435 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14436 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14437 final double[] bias1 = calibrator.getInitialBias();
14438 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
14439 final double[] bias2 = new double[3];
14440 calibrator.getInitialBias(bias2);
14441 assertArrayEquals(bias1, bias2, 0.0);
14442 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14443 assertEquals(b1, ba);
14444 final Matrix b2 = new Matrix(3, 1);
14445 calibrator.getInitialBiasAsMatrix(b2);
14446 assertEquals(b1, b2);
14447 final Matrix ma1 = new Matrix(3, 3);
14448 ma1.setSubmatrix(0, 0,
14449 2, 2,
14450 new double[]{ sx, myx, mzx,
14451 mxy, sy, mzy,
14452 mxz, myz, sz});
14453 assertEquals(calibrator.getInitialMa(), ma1);
14454 final Matrix ma2 = new Matrix(3, 3);
14455 calibrator.getInitialMa(ma2);
14456 assertEquals(ma1, ma2);
14457 assertSame(calibrator.getEcefPosition(), ecefPosition);
14458 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14459 final NEDPosition nedPosition1 = new NEDPosition();
14460 assertTrue(calibrator.getNedPosition(nedPosition1));
14461 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14462 assertSame(calibrator.getMeasurements(), measurements);
14463 assertTrue(calibrator.isCommonAxisUsed());
14464 assertSame(calibrator.getListener(), this);
14465 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14466 assertFalse(calibrator.isReady());
14467 assertFalse(calibrator.isRunning());
14468 assertNull(calibrator.getEstimatedBiases());
14469 assertFalse(calibrator.getEstimatedBiases(null));
14470 assertNull(calibrator.getEstimatedBiasesAsMatrix());
14471 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14472 assertNull(calibrator.getEstimatedBiasFx());
14473 assertNull(calibrator.getEstimatedBiasFy());
14474 assertNull(calibrator.getEstimatedBiasFz());
14475 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14476 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14477 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14478 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14479 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14480 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14481 assertNull(calibrator.getEstimatedMa());
14482 assertNull(calibrator.getEstimatedSx());
14483 assertNull(calibrator.getEstimatedSy());
14484 assertNull(calibrator.getEstimatedSz());
14485 assertNull(calibrator.getEstimatedMxy());
14486 assertNull(calibrator.getEstimatedMxz());
14487 assertNull(calibrator.getEstimatedMyx());
14488 assertNull(calibrator.getEstimatedMyz());
14489 assertNull(calibrator.getEstimatedMzx());
14490 assertNull(calibrator.getEstimatedMzy());
14491 assertNull(calibrator.getEstimatedCovariance());
14492 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14493 }
14494
14495 @Test
14496 public void testConstructor125() throws WrongSizeException {
14497 final Matrix ba = generateBa();
14498 final double biasX = ba.getElementAtIndex(0);
14499 final double biasY = ba.getElementAtIndex(1);
14500 final double biasZ = ba.getElementAtIndex(2);
14501
14502 final Acceleration bx = new Acceleration(biasX,
14503 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14504 final Acceleration by = new Acceleration(biasY,
14505 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14506 final Acceleration bz = new Acceleration(biasZ,
14507 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14508
14509 final Matrix ma = generateMaCommonAxis();
14510 final double sx = ma.getElementAt(0, 0);
14511 final double sy = ma.getElementAt(1, 1);
14512 final double sz = ma.getElementAt(2, 2);
14513 final double mxy = ma.getElementAt(0, 1);
14514 final double mxz = ma.getElementAt(0, 2);
14515 final double myx = ma.getElementAt(1, 0);
14516 final double myz = ma.getElementAt(1, 2);
14517 final double mzx = ma.getElementAt(2, 0);
14518 final double mzy = ma.getElementAt(2, 1);
14519
14520 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14521 final double latitude = Math.toRadians(
14522 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14523 final double longitude = Math.toRadians(
14524 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14525 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14526 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14527 final NEDVelocity nedVelocity = new NEDVelocity();
14528 final ECEFPosition ecefPosition = new ECEFPosition();
14529 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14530 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14531 ecefPosition, ecefVelocity);
14532
14533 final KnownPositionAccelerometerCalibrator calibrator =
14534 new KnownPositionAccelerometerCalibrator(ecefPosition,
14535 bx, by, bz, sx, sy, sz, mxy, mxz,
14536 myx, myz, mzx, mzy);
14537
14538
14539 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14540 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14541 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14542 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14543 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14544 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14545 final Acceleration bx2 = new Acceleration(0.0,
14546 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14547 calibrator.getInitialBiasXAsAcceleration(bx2);
14548 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14549 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14550 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14551 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14552 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14553 final Acceleration by2 = new Acceleration(0.0,
14554 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14555 calibrator.getInitialBiasYAsAcceleration(by2);
14556 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14557 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14558 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14559 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14560 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14561 final Acceleration bz2 = new Acceleration(0.0,
14562 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14563 calibrator.getInitialBiasZAsAcceleration(bz2);
14564 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14565 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14566 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14567 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14568 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14569 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14570 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14571 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14572 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14573 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14574 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14575 final double[] bias1 = calibrator.getInitialBias();
14576 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
14577 final double[] bias2 = new double[3];
14578 calibrator.getInitialBias(bias2);
14579 assertArrayEquals(bias1, bias2, 0.0);
14580 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14581 assertEquals(b1, ba);
14582 final Matrix b2 = new Matrix(3, 1);
14583 calibrator.getInitialBiasAsMatrix(b2);
14584 assertEquals(b1, b2);
14585 final Matrix ma1 = new Matrix(3, 3);
14586 ma1.setSubmatrix(0, 0,
14587 2, 2,
14588 new double[]{ sx, myx, mzx,
14589 mxy, sy, mzy,
14590 mxz, myz, sz});
14591 assertEquals(calibrator.getInitialMa(), ma1);
14592 final Matrix ma2 = new Matrix(3, 3);
14593 calibrator.getInitialMa(ma2);
14594 assertEquals(ma1, ma2);
14595 assertSame(calibrator.getEcefPosition(), ecefPosition);
14596 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14597 final NEDPosition nedPosition1 = new NEDPosition();
14598 assertTrue(calibrator.getNedPosition(nedPosition1));
14599 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14600 assertNull(calibrator.getMeasurements());
14601 assertFalse(calibrator.isCommonAxisUsed());
14602 assertNull(calibrator.getListener());
14603 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
14604 assertFalse(calibrator.isReady());
14605 assertFalse(calibrator.isRunning());
14606 assertNull(calibrator.getEstimatedBiases());
14607 assertFalse(calibrator.getEstimatedBiases(null));
14608 assertNull(calibrator.getEstimatedBiasesAsMatrix());
14609 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14610 assertNull(calibrator.getEstimatedBiasFx());
14611 assertNull(calibrator.getEstimatedBiasFy());
14612 assertNull(calibrator.getEstimatedBiasFz());
14613 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14614 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14615 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14616 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14617 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14618 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14619 assertNull(calibrator.getEstimatedMa());
14620 assertNull(calibrator.getEstimatedSx());
14621 assertNull(calibrator.getEstimatedSy());
14622 assertNull(calibrator.getEstimatedSz());
14623 assertNull(calibrator.getEstimatedMxy());
14624 assertNull(calibrator.getEstimatedMxz());
14625 assertNull(calibrator.getEstimatedMyx());
14626 assertNull(calibrator.getEstimatedMyz());
14627 assertNull(calibrator.getEstimatedMzx());
14628 assertNull(calibrator.getEstimatedMzy());
14629 assertNull(calibrator.getEstimatedCovariance());
14630 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14631 }
14632
14633 @Test
14634 public void testConstructor126() throws WrongSizeException {
14635 final Matrix ba = generateBa();
14636 final double biasX = ba.getElementAtIndex(0);
14637 final double biasY = ba.getElementAtIndex(1);
14638 final double biasZ = ba.getElementAtIndex(2);
14639
14640 final Acceleration bx = new Acceleration(biasX,
14641 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14642 final Acceleration by = new Acceleration(biasY,
14643 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14644 final Acceleration bz = new Acceleration(biasZ,
14645 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14646
14647 final Matrix ma = generateMaCommonAxis();
14648 final double sx = ma.getElementAt(0, 0);
14649 final double sy = ma.getElementAt(1, 1);
14650 final double sz = ma.getElementAt(2, 2);
14651 final double mxy = ma.getElementAt(0, 1);
14652 final double mxz = ma.getElementAt(0, 2);
14653 final double myx = ma.getElementAt(1, 0);
14654 final double myz = ma.getElementAt(1, 2);
14655 final double mzx = ma.getElementAt(2, 0);
14656 final double mzy = ma.getElementAt(2, 1);
14657
14658 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14659 final double latitude = Math.toRadians(
14660 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14661 final double longitude = Math.toRadians(
14662 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14663 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14664 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14665 final NEDVelocity nedVelocity = new NEDVelocity();
14666 final ECEFPosition ecefPosition = new ECEFPosition();
14667 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14668 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14669 ecefPosition, ecefVelocity);
14670
14671 final KnownPositionAccelerometerCalibrator calibrator =
14672 new KnownPositionAccelerometerCalibrator(ecefPosition,
14673 bx, by, bz, sx, sy, sz, mxy, mxz,
14674 myx, myz, mzx, mzy, this);
14675
14676
14677 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14678 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14679 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14680 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14681 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14682 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14683 final Acceleration bx2 = new Acceleration(0.0,
14684 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14685 calibrator.getInitialBiasXAsAcceleration(bx2);
14686 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14687 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14688 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14689 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14690 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14691 final Acceleration by2 = new Acceleration(0.0,
14692 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14693 calibrator.getInitialBiasYAsAcceleration(by2);
14694 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14695 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14696 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14697 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14698 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14699 final Acceleration bz2 = new Acceleration(0.0,
14700 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14701 calibrator.getInitialBiasZAsAcceleration(bz2);
14702 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14703 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14704 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14705 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14706 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14707 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14708 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14709 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14710 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14711 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14712 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14713 final double[] bias1 = calibrator.getInitialBias();
14714 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
14715 final double[] bias2 = new double[3];
14716 calibrator.getInitialBias(bias2);
14717 assertArrayEquals(bias1, bias2, 0.0);
14718 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14719 assertEquals(b1, ba);
14720 final Matrix b2 = new Matrix(3, 1);
14721 calibrator.getInitialBiasAsMatrix(b2);
14722 assertEquals(b1, b2);
14723 final Matrix ma1 = new Matrix(3, 3);
14724 ma1.setSubmatrix(0, 0,
14725 2, 2,
14726 new double[]{ sx, myx, mzx,
14727 mxy, sy, mzy,
14728 mxz, myz, sz});
14729 assertEquals(calibrator.getInitialMa(), ma1);
14730 final Matrix ma2 = new Matrix(3, 3);
14731 calibrator.getInitialMa(ma2);
14732 assertEquals(ma1, ma2);
14733 assertSame(calibrator.getEcefPosition(), ecefPosition);
14734 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14735 final NEDPosition nedPosition1 = new NEDPosition();
14736 assertTrue(calibrator.getNedPosition(nedPosition1));
14737 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14738 assertNull(calibrator.getMeasurements());
14739 assertFalse(calibrator.isCommonAxisUsed());
14740 assertSame(calibrator.getListener(), this);
14741 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
14742 assertFalse(calibrator.isReady());
14743 assertFalse(calibrator.isRunning());
14744 assertNull(calibrator.getEstimatedBiases());
14745 assertFalse(calibrator.getEstimatedBiases(null));
14746 assertNull(calibrator.getEstimatedBiasesAsMatrix());
14747 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14748 assertNull(calibrator.getEstimatedBiasFx());
14749 assertNull(calibrator.getEstimatedBiasFy());
14750 assertNull(calibrator.getEstimatedBiasFz());
14751 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14752 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14753 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14754 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14755 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14756 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14757 assertNull(calibrator.getEstimatedMa());
14758 assertNull(calibrator.getEstimatedSx());
14759 assertNull(calibrator.getEstimatedSy());
14760 assertNull(calibrator.getEstimatedSz());
14761 assertNull(calibrator.getEstimatedMxy());
14762 assertNull(calibrator.getEstimatedMxz());
14763 assertNull(calibrator.getEstimatedMyx());
14764 assertNull(calibrator.getEstimatedMyz());
14765 assertNull(calibrator.getEstimatedMzx());
14766 assertNull(calibrator.getEstimatedMzy());
14767 assertNull(calibrator.getEstimatedCovariance());
14768 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14769 }
14770
14771 @Test
14772 public void testConstructor127() throws WrongSizeException {
14773 final Collection<StandardDeviationBodyKinematics> measurements =
14774 Collections.emptyList();
14775
14776 final Matrix ba = generateBa();
14777 final double biasX = ba.getElementAtIndex(0);
14778 final double biasY = ba.getElementAtIndex(1);
14779 final double biasZ = ba.getElementAtIndex(2);
14780
14781 final Acceleration bx = new Acceleration(biasX,
14782 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14783 final Acceleration by = new Acceleration(biasY,
14784 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14785 final Acceleration bz = new Acceleration(biasZ,
14786 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14787
14788 final Matrix ma = generateMaCommonAxis();
14789 final double sx = ma.getElementAt(0, 0);
14790 final double sy = ma.getElementAt(1, 1);
14791 final double sz = ma.getElementAt(2, 2);
14792 final double mxy = ma.getElementAt(0, 1);
14793 final double mxz = ma.getElementAt(0, 2);
14794 final double myx = ma.getElementAt(1, 0);
14795 final double myz = ma.getElementAt(1, 2);
14796 final double mzx = ma.getElementAt(2, 0);
14797 final double mzy = ma.getElementAt(2, 1);
14798
14799 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14800 final double latitude = Math.toRadians(
14801 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14802 final double longitude = Math.toRadians(
14803 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14804 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14805 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14806 final NEDVelocity nedVelocity = new NEDVelocity();
14807 final ECEFPosition ecefPosition = new ECEFPosition();
14808 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14809 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14810 ecefPosition, ecefVelocity);
14811
14812 final KnownPositionAccelerometerCalibrator calibrator =
14813 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
14814 bx, by, bz, sx, sy, sz, mxy, mxz,
14815 myx, myz, mzx, mzy);
14816
14817
14818 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14819 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14820 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14821 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14822 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14823 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14824 final Acceleration bx2 = new Acceleration(0.0,
14825 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14826 calibrator.getInitialBiasXAsAcceleration(bx2);
14827 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14828 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14829 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14830 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14831 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14832 final Acceleration by2 = new Acceleration(0.0,
14833 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14834 calibrator.getInitialBiasYAsAcceleration(by2);
14835 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14836 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14837 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14838 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14839 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14840 final Acceleration bz2 = new Acceleration(0.0,
14841 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14842 calibrator.getInitialBiasZAsAcceleration(bz2);
14843 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14844 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14845 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14846 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14847 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14848 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14849 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14850 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14851 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14852 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14853 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14854 final double[] bias1 = calibrator.getInitialBias();
14855 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
14856 final double[] bias2 = new double[3];
14857 calibrator.getInitialBias(bias2);
14858 assertArrayEquals(bias1, bias2, 0.0);
14859 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14860 assertEquals(b1, ba);
14861 final Matrix b2 = new Matrix(3, 1);
14862 calibrator.getInitialBiasAsMatrix(b2);
14863 assertEquals(b1, b2);
14864 final Matrix ma1 = new Matrix(3, 3);
14865 ma1.setSubmatrix(0, 0,
14866 2, 2,
14867 new double[]{ sx, myx, mzx,
14868 mxy, sy, mzy,
14869 mxz, myz, sz});
14870 assertEquals(calibrator.getInitialMa(), ma1);
14871 final Matrix ma2 = new Matrix(3, 3);
14872 calibrator.getInitialMa(ma2);
14873 assertEquals(ma1, ma2);
14874 assertSame(calibrator.getEcefPosition(), ecefPosition);
14875 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14876 final NEDPosition nedPosition1 = new NEDPosition();
14877 assertTrue(calibrator.getNedPosition(nedPosition1));
14878 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14879 assertSame(calibrator.getMeasurements(), measurements);
14880 assertFalse(calibrator.isCommonAxisUsed());
14881 assertNull(calibrator.getListener());
14882 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
14883 assertFalse(calibrator.isReady());
14884 assertFalse(calibrator.isRunning());
14885 assertNull(calibrator.getEstimatedBiases());
14886 assertFalse(calibrator.getEstimatedBiases(null));
14887 assertNull(calibrator.getEstimatedBiasesAsMatrix());
14888 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14889 assertNull(calibrator.getEstimatedBiasFx());
14890 assertNull(calibrator.getEstimatedBiasFy());
14891 assertNull(calibrator.getEstimatedBiasFz());
14892 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14893 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14894 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14895 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14896 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14897 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14898 assertNull(calibrator.getEstimatedMa());
14899 assertNull(calibrator.getEstimatedSx());
14900 assertNull(calibrator.getEstimatedSy());
14901 assertNull(calibrator.getEstimatedSz());
14902 assertNull(calibrator.getEstimatedMxy());
14903 assertNull(calibrator.getEstimatedMxz());
14904 assertNull(calibrator.getEstimatedMyx());
14905 assertNull(calibrator.getEstimatedMyz());
14906 assertNull(calibrator.getEstimatedMzx());
14907 assertNull(calibrator.getEstimatedMzy());
14908 assertNull(calibrator.getEstimatedCovariance());
14909 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14910 }
14911
14912 @Test
14913 public void testConstructor128() throws WrongSizeException {
14914 final Collection<StandardDeviationBodyKinematics> measurements =
14915 Collections.emptyList();
14916
14917 final Matrix ba = generateBa();
14918 final double biasX = ba.getElementAtIndex(0);
14919 final double biasY = ba.getElementAtIndex(1);
14920 final double biasZ = ba.getElementAtIndex(2);
14921
14922 final Acceleration bx = new Acceleration(biasX,
14923 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14924 final Acceleration by = new Acceleration(biasY,
14925 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14926 final Acceleration bz = new Acceleration(biasZ,
14927 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14928
14929 final Matrix ma = generateMaCommonAxis();
14930 final double sx = ma.getElementAt(0, 0);
14931 final double sy = ma.getElementAt(1, 1);
14932 final double sz = ma.getElementAt(2, 2);
14933 final double mxy = ma.getElementAt(0, 1);
14934 final double mxz = ma.getElementAt(0, 2);
14935 final double myx = ma.getElementAt(1, 0);
14936 final double myz = ma.getElementAt(1, 2);
14937 final double mzx = ma.getElementAt(2, 0);
14938 final double mzy = ma.getElementAt(2, 1);
14939
14940 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14941 final double latitude = Math.toRadians(
14942 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14943 final double longitude = Math.toRadians(
14944 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14945 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14946 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14947 final NEDVelocity nedVelocity = new NEDVelocity();
14948 final ECEFPosition ecefPosition = new ECEFPosition();
14949 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14950 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14951 ecefPosition, ecefVelocity);
14952
14953 final KnownPositionAccelerometerCalibrator calibrator =
14954 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
14955 bx, by, bz, sx, sy, sz, mxy, mxz,
14956 myx, myz, mzx, mzy, this);
14957
14958
14959 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14960 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14961 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14962 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14963 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14964 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14965 final Acceleration bx2 = new Acceleration(0.0,
14966 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14967 calibrator.getInitialBiasXAsAcceleration(bx2);
14968 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14969 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14970 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14971 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14972 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14973 final Acceleration by2 = new Acceleration(0.0,
14974 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14975 calibrator.getInitialBiasYAsAcceleration(by2);
14976 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14977 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14978 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14979 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14980 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14981 final Acceleration bz2 = new Acceleration(0.0,
14982 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14983 calibrator.getInitialBiasZAsAcceleration(bz2);
14984 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14985 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14986 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14987 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14988 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14989 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14990 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14991 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14992 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14993 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14994 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14995 final double[] bias1 = calibrator.getInitialBias();
14996 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
14997 final double[] bias2 = new double[3];
14998 calibrator.getInitialBias(bias2);
14999 assertArrayEquals(bias1, bias2, 0.0);
15000 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15001 assertEquals(b1, ba);
15002 final Matrix b2 = new Matrix(3, 1);
15003 calibrator.getInitialBiasAsMatrix(b2);
15004 assertEquals(b1, b2);
15005 final Matrix ma1 = new Matrix(3, 3);
15006 ma1.setSubmatrix(0, 0,
15007 2, 2,
15008 new double[]{ sx, myx, mzx,
15009 mxy, sy, mzy,
15010 mxz, myz, sz});
15011 assertEquals(calibrator.getInitialMa(), ma1);
15012 final Matrix ma2 = new Matrix(3, 3);
15013 calibrator.getInitialMa(ma2);
15014 assertEquals(ma1, ma2);
15015 assertSame(calibrator.getEcefPosition(), ecefPosition);
15016 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15017 final NEDPosition nedPosition1 = new NEDPosition();
15018 assertTrue(calibrator.getNedPosition(nedPosition1));
15019 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15020 assertSame(calibrator.getMeasurements(), measurements);
15021 assertFalse(calibrator.isCommonAxisUsed());
15022 assertSame(calibrator.getListener(), this);
15023 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15024 assertFalse(calibrator.isReady());
15025 assertFalse(calibrator.isRunning());
15026 assertNull(calibrator.getEstimatedBiases());
15027 assertFalse(calibrator.getEstimatedBiases(null));
15028 assertNull(calibrator.getEstimatedBiasesAsMatrix());
15029 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15030 assertNull(calibrator.getEstimatedBiasFx());
15031 assertNull(calibrator.getEstimatedBiasFy());
15032 assertNull(calibrator.getEstimatedBiasFz());
15033 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15034 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15035 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15036 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15037 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15038 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15039 assertNull(calibrator.getEstimatedMa());
15040 assertNull(calibrator.getEstimatedSx());
15041 assertNull(calibrator.getEstimatedSy());
15042 assertNull(calibrator.getEstimatedSz());
15043 assertNull(calibrator.getEstimatedMxy());
15044 assertNull(calibrator.getEstimatedMxz());
15045 assertNull(calibrator.getEstimatedMyx());
15046 assertNull(calibrator.getEstimatedMyz());
15047 assertNull(calibrator.getEstimatedMzx());
15048 assertNull(calibrator.getEstimatedMzy());
15049 assertNull(calibrator.getEstimatedCovariance());
15050 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15051 }
15052
15053 @Test
15054 public void testConstructor129() throws WrongSizeException {
15055 final Matrix ba = generateBa();
15056 final double biasX = ba.getElementAtIndex(0);
15057 final double biasY = ba.getElementAtIndex(1);
15058 final double biasZ = ba.getElementAtIndex(2);
15059
15060 final Acceleration bx = new Acceleration(biasX,
15061 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15062 final Acceleration by = new Acceleration(biasY,
15063 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15064 final Acceleration bz = new Acceleration(biasZ,
15065 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15066
15067 final Matrix ma = generateMaCommonAxis();
15068 final double sx = ma.getElementAt(0, 0);
15069 final double sy = ma.getElementAt(1, 1);
15070 final double sz = ma.getElementAt(2, 2);
15071 final double mxy = ma.getElementAt(0, 1);
15072 final double mxz = ma.getElementAt(0, 2);
15073 final double myx = ma.getElementAt(1, 0);
15074 final double myz = ma.getElementAt(1, 2);
15075 final double mzx = ma.getElementAt(2, 0);
15076 final double mzy = ma.getElementAt(2, 1);
15077
15078 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15079 final double latitude = Math.toRadians(
15080 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15081 final double longitude = Math.toRadians(
15082 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15083 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15084 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15085 final NEDVelocity nedVelocity = new NEDVelocity();
15086 final ECEFPosition ecefPosition = new ECEFPosition();
15087 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15088 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15089 ecefPosition, ecefVelocity);
15090
15091 final KnownPositionAccelerometerCalibrator calibrator =
15092 new KnownPositionAccelerometerCalibrator(ecefPosition,
15093 true, bx, by, bz, sx, sy, sz, mxy, mxz,
15094 myx, myz, mzx, mzy);
15095
15096
15097 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15098 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15099 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15100 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15101 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15102 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15103 final Acceleration bx2 = new Acceleration(0.0,
15104 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15105 calibrator.getInitialBiasXAsAcceleration(bx2);
15106 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15107 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15108 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15109 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15110 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15111 final Acceleration by2 = new Acceleration(0.0,
15112 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15113 calibrator.getInitialBiasYAsAcceleration(by2);
15114 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15115 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15116 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15117 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15118 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15119 final Acceleration bz2 = new Acceleration(0.0,
15120 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15121 calibrator.getInitialBiasZAsAcceleration(bz2);
15122 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15123 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15124 assertEquals(calibrator.getInitialSx(), sx, 0.0);
15125 assertEquals(calibrator.getInitialSy(), sy, 0.0);
15126 assertEquals(calibrator.getInitialSz(), sz, 0.0);
15127 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15128 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15129 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15130 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15131 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15132 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15133 final double[] bias1 = calibrator.getInitialBias();
15134 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
15135 final double[] bias2 = new double[3];
15136 calibrator.getInitialBias(bias2);
15137 assertArrayEquals(bias1, bias2, 0.0);
15138 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15139 assertEquals(b1, ba);
15140 final Matrix b2 = new Matrix(3, 1);
15141 calibrator.getInitialBiasAsMatrix(b2);
15142 assertEquals(b1, b2);
15143 final Matrix ma1 = new Matrix(3, 3);
15144 ma1.setSubmatrix(0, 0,
15145 2, 2,
15146 new double[]{ sx, myx, mzx,
15147 mxy, sy, mzy,
15148 mxz, myz, sz});
15149 assertEquals(calibrator.getInitialMa(), ma1);
15150 final Matrix ma2 = new Matrix(3, 3);
15151 calibrator.getInitialMa(ma2);
15152 assertEquals(ma1, ma2);
15153 assertSame(calibrator.getEcefPosition(), ecefPosition);
15154 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15155 final NEDPosition nedPosition1 = new NEDPosition();
15156 assertTrue(calibrator.getNedPosition(nedPosition1));
15157 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15158 assertNull(calibrator.getMeasurements());
15159 assertTrue(calibrator.isCommonAxisUsed());
15160 assertNull(calibrator.getListener());
15161 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15162 assertFalse(calibrator.isReady());
15163 assertFalse(calibrator.isRunning());
15164 assertNull(calibrator.getEstimatedBiases());
15165 assertFalse(calibrator.getEstimatedBiases(null));
15166 assertNull(calibrator.getEstimatedBiasesAsMatrix());
15167 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15168 assertNull(calibrator.getEstimatedBiasFx());
15169 assertNull(calibrator.getEstimatedBiasFy());
15170 assertNull(calibrator.getEstimatedBiasFz());
15171 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15172 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15173 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15174 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15175 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15176 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15177 assertNull(calibrator.getEstimatedMa());
15178 assertNull(calibrator.getEstimatedSx());
15179 assertNull(calibrator.getEstimatedSy());
15180 assertNull(calibrator.getEstimatedSz());
15181 assertNull(calibrator.getEstimatedMxy());
15182 assertNull(calibrator.getEstimatedMxz());
15183 assertNull(calibrator.getEstimatedMyx());
15184 assertNull(calibrator.getEstimatedMyz());
15185 assertNull(calibrator.getEstimatedMzx());
15186 assertNull(calibrator.getEstimatedMzy());
15187 assertNull(calibrator.getEstimatedCovariance());
15188 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15189 }
15190
15191 @Test
15192 public void testConstructor130() throws WrongSizeException {
15193 final Matrix ba = generateBa();
15194 final double biasX = ba.getElementAtIndex(0);
15195 final double biasY = ba.getElementAtIndex(1);
15196 final double biasZ = ba.getElementAtIndex(2);
15197
15198 final Acceleration bx = new Acceleration(biasX,
15199 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15200 final Acceleration by = new Acceleration(biasY,
15201 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15202 final Acceleration bz = new Acceleration(biasZ,
15203 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15204
15205 final Matrix ma = generateMaCommonAxis();
15206 final double sx = ma.getElementAt(0, 0);
15207 final double sy = ma.getElementAt(1, 1);
15208 final double sz = ma.getElementAt(2, 2);
15209 final double mxy = ma.getElementAt(0, 1);
15210 final double mxz = ma.getElementAt(0, 2);
15211 final double myx = ma.getElementAt(1, 0);
15212 final double myz = ma.getElementAt(1, 2);
15213 final double mzx = ma.getElementAt(2, 0);
15214 final double mzy = ma.getElementAt(2, 1);
15215
15216 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15217 final double latitude = Math.toRadians(
15218 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15219 final double longitude = Math.toRadians(
15220 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15221 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15222 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15223 final NEDVelocity nedVelocity = new NEDVelocity();
15224 final ECEFPosition ecefPosition = new ECEFPosition();
15225 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15226 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15227 ecefPosition, ecefVelocity);
15228
15229 final KnownPositionAccelerometerCalibrator calibrator =
15230 new KnownPositionAccelerometerCalibrator(ecefPosition,
15231 true, bx, by, bz, sx, sy, sz, mxy, mxz,
15232 myx, myz, mzx, mzy, this);
15233
15234
15235 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15236 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15237 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15238 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15239 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15240 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15241 final Acceleration bx2 = new Acceleration(0.0,
15242 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15243 calibrator.getInitialBiasXAsAcceleration(bx2);
15244 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15245 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15246 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15247 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15248 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15249 final Acceleration by2 = new Acceleration(0.0,
15250 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15251 calibrator.getInitialBiasYAsAcceleration(by2);
15252 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15253 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15254 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15255 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15256 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15257 final Acceleration bz2 = new Acceleration(0.0,
15258 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15259 calibrator.getInitialBiasZAsAcceleration(bz2);
15260 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15261 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15262 assertEquals(calibrator.getInitialSx(), sx, 0.0);
15263 assertEquals(calibrator.getInitialSy(), sy, 0.0);
15264 assertEquals(calibrator.getInitialSz(), sz, 0.0);
15265 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15266 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15267 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15268 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15269 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15270 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15271 final double[] bias1 = calibrator.getInitialBias();
15272 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
15273 final double[] bias2 = new double[3];
15274 calibrator.getInitialBias(bias2);
15275 assertArrayEquals(bias1, bias2, 0.0);
15276 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15277 assertEquals(b1, ba);
15278 final Matrix b2 = new Matrix(3, 1);
15279 calibrator.getInitialBiasAsMatrix(b2);
15280 assertEquals(b1, b2);
15281 final Matrix ma1 = new Matrix(3, 3);
15282 ma1.setSubmatrix(0, 0,
15283 2, 2,
15284 new double[]{ sx, myx, mzx,
15285 mxy, sy, mzy,
15286 mxz, myz, sz});
15287 assertEquals(calibrator.getInitialMa(), ma1);
15288 final Matrix ma2 = new Matrix(3, 3);
15289 calibrator.getInitialMa(ma2);
15290 assertEquals(ma1, ma2);
15291 assertSame(calibrator.getEcefPosition(), ecefPosition);
15292 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15293 final NEDPosition nedPosition1 = new NEDPosition();
15294 assertTrue(calibrator.getNedPosition(nedPosition1));
15295 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15296 assertNull(calibrator.getMeasurements());
15297 assertTrue(calibrator.isCommonAxisUsed());
15298 assertSame(calibrator.getListener(), this);
15299 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15300 assertFalse(calibrator.isReady());
15301 assertFalse(calibrator.isRunning());
15302 assertNull(calibrator.getEstimatedBiases());
15303 assertFalse(calibrator.getEstimatedBiases(null));
15304 assertNull(calibrator.getEstimatedBiasesAsMatrix());
15305 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15306 assertNull(calibrator.getEstimatedBiasFx());
15307 assertNull(calibrator.getEstimatedBiasFy());
15308 assertNull(calibrator.getEstimatedBiasFz());
15309 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15310 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15311 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15312 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15313 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15314 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15315 assertNull(calibrator.getEstimatedMa());
15316 assertNull(calibrator.getEstimatedSx());
15317 assertNull(calibrator.getEstimatedSy());
15318 assertNull(calibrator.getEstimatedSz());
15319 assertNull(calibrator.getEstimatedMxy());
15320 assertNull(calibrator.getEstimatedMxz());
15321 assertNull(calibrator.getEstimatedMyx());
15322 assertNull(calibrator.getEstimatedMyz());
15323 assertNull(calibrator.getEstimatedMzx());
15324 assertNull(calibrator.getEstimatedMzy());
15325 assertNull(calibrator.getEstimatedCovariance());
15326 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15327 }
15328
15329 @Test
15330 public void testConstructor131() throws WrongSizeException {
15331 final Collection<StandardDeviationBodyKinematics> measurements =
15332 Collections.emptyList();
15333
15334 final Matrix ba = generateBa();
15335 final double biasX = ba.getElementAtIndex(0);
15336 final double biasY = ba.getElementAtIndex(1);
15337 final double biasZ = ba.getElementAtIndex(2);
15338
15339 final Acceleration bx = new Acceleration(biasX,
15340 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15341 final Acceleration by = new Acceleration(biasY,
15342 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15343 final Acceleration bz = new Acceleration(biasZ,
15344 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15345
15346 final Matrix ma = generateMaCommonAxis();
15347 final double sx = ma.getElementAt(0, 0);
15348 final double sy = ma.getElementAt(1, 1);
15349 final double sz = ma.getElementAt(2, 2);
15350 final double mxy = ma.getElementAt(0, 1);
15351 final double mxz = ma.getElementAt(0, 2);
15352 final double myx = ma.getElementAt(1, 0);
15353 final double myz = ma.getElementAt(1, 2);
15354 final double mzx = ma.getElementAt(2, 0);
15355 final double mzy = ma.getElementAt(2, 1);
15356
15357 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15358 final double latitude = Math.toRadians(
15359 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15360 final double longitude = Math.toRadians(
15361 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15362 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15363 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15364 final NEDVelocity nedVelocity = new NEDVelocity();
15365 final ECEFPosition ecefPosition = new ECEFPosition();
15366 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15367 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15368 ecefPosition, ecefVelocity);
15369
15370 final KnownPositionAccelerometerCalibrator calibrator =
15371 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
15372 true, bx, by, bz, sx, sy, sz, mxy, mxz,
15373 myx, myz, mzx, mzy);
15374
15375
15376 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15377 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15378 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15379 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15380 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15381 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15382 final Acceleration bx2 = new Acceleration(0.0,
15383 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15384 calibrator.getInitialBiasXAsAcceleration(bx2);
15385 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15386 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15387 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15388 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15389 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15390 final Acceleration by2 = new Acceleration(0.0,
15391 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15392 calibrator.getInitialBiasYAsAcceleration(by2);
15393 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15394 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15395 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15396 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15397 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15398 final Acceleration bz2 = new Acceleration(0.0,
15399 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15400 calibrator.getInitialBiasZAsAcceleration(bz2);
15401 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15402 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15403 assertEquals(calibrator.getInitialSx(), sx, 0.0);
15404 assertEquals(calibrator.getInitialSy(), sy, 0.0);
15405 assertEquals(calibrator.getInitialSz(), sz, 0.0);
15406 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15407 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15408 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15409 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15410 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15411 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15412 final double[] bias1 = calibrator.getInitialBias();
15413 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
15414 final double[] bias2 = new double[3];
15415 calibrator.getInitialBias(bias2);
15416 assertArrayEquals(bias1, bias2, 0.0);
15417 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15418 assertEquals(b1, ba);
15419 final Matrix b2 = new Matrix(3, 1);
15420 calibrator.getInitialBiasAsMatrix(b2);
15421 assertEquals(b1, b2);
15422 final Matrix ma1 = new Matrix(3, 3);
15423 ma1.setSubmatrix(0, 0,
15424 2, 2,
15425 new double[]{ sx, myx, mzx,
15426 mxy, sy, mzy,
15427 mxz, myz, sz});
15428 assertEquals(calibrator.getInitialMa(), ma1);
15429 final Matrix ma2 = new Matrix(3, 3);
15430 calibrator.getInitialMa(ma2);
15431 assertEquals(ma1, ma2);
15432 assertSame(calibrator.getEcefPosition(), ecefPosition);
15433 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15434 final NEDPosition nedPosition1 = new NEDPosition();
15435 assertTrue(calibrator.getNedPosition(nedPosition1));
15436 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15437 assertSame(calibrator.getMeasurements(), measurements);
15438 assertTrue(calibrator.isCommonAxisUsed());
15439 assertNull(calibrator.getListener());
15440 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15441 assertFalse(calibrator.isReady());
15442 assertFalse(calibrator.isRunning());
15443 assertNull(calibrator.getEstimatedBiases());
15444 assertFalse(calibrator.getEstimatedBiases(null));
15445 assertNull(calibrator.getEstimatedBiasesAsMatrix());
15446 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15447 assertNull(calibrator.getEstimatedBiasFx());
15448 assertNull(calibrator.getEstimatedBiasFy());
15449 assertNull(calibrator.getEstimatedBiasFz());
15450 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15451 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15452 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15453 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15454 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15455 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15456 assertNull(calibrator.getEstimatedMa());
15457 assertNull(calibrator.getEstimatedSx());
15458 assertNull(calibrator.getEstimatedSy());
15459 assertNull(calibrator.getEstimatedSz());
15460 assertNull(calibrator.getEstimatedMxy());
15461 assertNull(calibrator.getEstimatedMxz());
15462 assertNull(calibrator.getEstimatedMyx());
15463 assertNull(calibrator.getEstimatedMyz());
15464 assertNull(calibrator.getEstimatedMzx());
15465 assertNull(calibrator.getEstimatedMzy());
15466 assertNull(calibrator.getEstimatedCovariance());
15467 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15468 }
15469
15470 @Test
15471 public void testConstructor132() throws WrongSizeException {
15472 final Collection<StandardDeviationBodyKinematics> measurements =
15473 Collections.emptyList();
15474
15475 final Matrix ba = generateBa();
15476 final double biasX = ba.getElementAtIndex(0);
15477 final double biasY = ba.getElementAtIndex(1);
15478 final double biasZ = ba.getElementAtIndex(2);
15479
15480 final Acceleration bx = new Acceleration(biasX,
15481 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15482 final Acceleration by = new Acceleration(biasY,
15483 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15484 final Acceleration bz = new Acceleration(biasZ,
15485 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15486
15487 final Matrix ma = generateMaCommonAxis();
15488 final double sx = ma.getElementAt(0, 0);
15489 final double sy = ma.getElementAt(1, 1);
15490 final double sz = ma.getElementAt(2, 2);
15491 final double mxy = ma.getElementAt(0, 1);
15492 final double mxz = ma.getElementAt(0, 2);
15493 final double myx = ma.getElementAt(1, 0);
15494 final double myz = ma.getElementAt(1, 2);
15495 final double mzx = ma.getElementAt(2, 0);
15496 final double mzy = ma.getElementAt(2, 1);
15497
15498 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15499 final double latitude = Math.toRadians(
15500 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15501 final double longitude = Math.toRadians(
15502 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15503 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15504 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15505 final NEDVelocity nedVelocity = new NEDVelocity();
15506 final ECEFPosition ecefPosition = new ECEFPosition();
15507 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15508 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15509 ecefPosition, ecefVelocity);
15510
15511 final KnownPositionAccelerometerCalibrator calibrator =
15512 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
15513 true, bx, by, bz, sx, sy, sz, mxy, mxz,
15514 myx, myz, mzx, mzy, this);
15515
15516
15517 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15518 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15519 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15520 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15521 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15522 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15523 final Acceleration bx2 = new Acceleration(0.0,
15524 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15525 calibrator.getInitialBiasXAsAcceleration(bx2);
15526 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15527 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15528 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15529 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15530 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15531 final Acceleration by2 = new Acceleration(0.0,
15532 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15533 calibrator.getInitialBiasYAsAcceleration(by2);
15534 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15535 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15536 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15537 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15538 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15539 final Acceleration bz2 = new Acceleration(0.0,
15540 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15541 calibrator.getInitialBiasZAsAcceleration(bz2);
15542 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15543 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15544 assertEquals(calibrator.getInitialSx(), sx, 0.0);
15545 assertEquals(calibrator.getInitialSy(), sy, 0.0);
15546 assertEquals(calibrator.getInitialSz(), sz, 0.0);
15547 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15548 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15549 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15550 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15551 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15552 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15553 final double[] bias1 = calibrator.getInitialBias();
15554 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
15555 final double[] bias2 = new double[3];
15556 calibrator.getInitialBias(bias2);
15557 assertArrayEquals(bias1, bias2, 0.0);
15558 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15559 assertEquals(b1, ba);
15560 final Matrix b2 = new Matrix(3, 1);
15561 calibrator.getInitialBiasAsMatrix(b2);
15562 assertEquals(b1, b2);
15563 final Matrix ma1 = new Matrix(3, 3);
15564 ma1.setSubmatrix(0, 0,
15565 2, 2,
15566 new double[]{ sx, myx, mzx,
15567 mxy, sy, mzy,
15568 mxz, myz, sz});
15569 assertEquals(calibrator.getInitialMa(), ma1);
15570 final Matrix ma2 = new Matrix(3, 3);
15571 calibrator.getInitialMa(ma2);
15572 assertEquals(ma1, ma2);
15573 assertSame(calibrator.getEcefPosition(), ecefPosition);
15574 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15575 final NEDPosition nedPosition1 = new NEDPosition();
15576 assertTrue(calibrator.getNedPosition(nedPosition1));
15577 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15578 assertSame(calibrator.getMeasurements(), measurements);
15579 assertTrue(calibrator.isCommonAxisUsed());
15580 assertSame(calibrator.getListener(), this);
15581 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15582 assertFalse(calibrator.isReady());
15583 assertFalse(calibrator.isRunning());
15584 assertNull(calibrator.getEstimatedBiases());
15585 assertFalse(calibrator.getEstimatedBiases(null));
15586 assertNull(calibrator.getEstimatedBiasesAsMatrix());
15587 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15588 assertNull(calibrator.getEstimatedBiasFx());
15589 assertNull(calibrator.getEstimatedBiasFy());
15590 assertNull(calibrator.getEstimatedBiasFz());
15591 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15592 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15593 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15594 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15595 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15596 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15597 assertNull(calibrator.getEstimatedMa());
15598 assertNull(calibrator.getEstimatedSx());
15599 assertNull(calibrator.getEstimatedSy());
15600 assertNull(calibrator.getEstimatedSz());
15601 assertNull(calibrator.getEstimatedMxy());
15602 assertNull(calibrator.getEstimatedMxz());
15603 assertNull(calibrator.getEstimatedMyx());
15604 assertNull(calibrator.getEstimatedMyz());
15605 assertNull(calibrator.getEstimatedMzx());
15606 assertNull(calibrator.getEstimatedMzy());
15607 assertNull(calibrator.getEstimatedCovariance());
15608 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15609 }
15610
15611 @Test
15612 public void testConstructor133() throws WrongSizeException {
15613 final Matrix ba = generateBa();
15614 final double[] bias = ba.getBuffer();
15615 final double biasX = ba.getElementAtIndex(0);
15616 final double biasY = ba.getElementAtIndex(1);
15617 final double biasZ = ba.getElementAtIndex(2);
15618
15619 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15620 final double latitude = Math.toRadians(
15621 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15622 final double longitude = Math.toRadians(
15623 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15624 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15625 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15626 final NEDVelocity nedVelocity = new NEDVelocity();
15627 final ECEFPosition ecefPosition = new ECEFPosition();
15628 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15629 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15630 ecefPosition, ecefVelocity);
15631
15632 KnownPositionAccelerometerCalibrator calibrator =
15633 new KnownPositionAccelerometerCalibrator(ecefPosition, bias);
15634
15635
15636 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15637 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15638 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15639 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15640 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15641 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15642 final Acceleration bx2 = new Acceleration(0.0,
15643 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15644 calibrator.getInitialBiasXAsAcceleration(bx2);
15645 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15646 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15647 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15648 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15649 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15650 final Acceleration by2 = new Acceleration(0.0,
15651 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15652 calibrator.getInitialBiasYAsAcceleration(by2);
15653 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15654 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15655 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15656 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15657 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15658 final Acceleration bz2 = new Acceleration(0.0,
15659 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15660 calibrator.getInitialBiasZAsAcceleration(bz2);
15661 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15662 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15663 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
15664 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
15665 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
15666 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
15667 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
15668 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
15669 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
15670 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
15671 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
15672 final double[] bias1 = calibrator.getInitialBias();
15673 assertArrayEquals(bias1, bias, 0.0);
15674 final double[] bias2 = new double[3];
15675 calibrator.getInitialBias(bias2);
15676 assertArrayEquals(bias1, bias2, 0.0);
15677 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15678 assertEquals(b1, ba);
15679 final Matrix b2 = new Matrix(3, 1);
15680 calibrator.getInitialBiasAsMatrix(b2);
15681 assertEquals(b1, b2);
15682 final Matrix ma1 = calibrator.getInitialMa();
15683 assertEquals(ma1, new Matrix(3, 3));
15684 final Matrix ma2 = new Matrix(3, 3);
15685 calibrator.getInitialMa(ma2);
15686 assertEquals(ma1, ma2);
15687 assertSame(calibrator.getEcefPosition(), ecefPosition);
15688 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15689 final NEDPosition nedPosition1 = new NEDPosition();
15690 assertTrue(calibrator.getNedPosition(nedPosition1));
15691 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15692 assertNull(calibrator.getMeasurements());
15693 assertFalse(calibrator.isCommonAxisUsed());
15694 assertNull(calibrator.getListener());
15695 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15696 assertFalse(calibrator.isReady());
15697 assertFalse(calibrator.isRunning());
15698 assertNull(calibrator.getEstimatedBiases());
15699 assertFalse(calibrator.getEstimatedBiases(null));
15700 assertNull(calibrator.getEstimatedBiasesAsMatrix());
15701 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15702 assertNull(calibrator.getEstimatedBiasFx());
15703 assertNull(calibrator.getEstimatedBiasFy());
15704 assertNull(calibrator.getEstimatedBiasFz());
15705 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15706 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15707 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15708 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15709 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15710 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15711 assertNull(calibrator.getEstimatedMa());
15712 assertNull(calibrator.getEstimatedSx());
15713 assertNull(calibrator.getEstimatedSy());
15714 assertNull(calibrator.getEstimatedSz());
15715 assertNull(calibrator.getEstimatedMxy());
15716 assertNull(calibrator.getEstimatedMxz());
15717 assertNull(calibrator.getEstimatedMyx());
15718 assertNull(calibrator.getEstimatedMyz());
15719 assertNull(calibrator.getEstimatedMzx());
15720 assertNull(calibrator.getEstimatedMzy());
15721 assertNull(calibrator.getEstimatedCovariance());
15722 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15723
15724
15725 calibrator = null;
15726 try {
15727 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
15728 new double[1]);
15729 fail("IllegalArgumentException expected but not thrown");
15730 } catch (final IllegalArgumentException ignore) {
15731 }
15732 assertNull(calibrator);
15733 }
15734
15735 @Test
15736 public void testConstructor134() throws WrongSizeException {
15737 final Matrix ba = generateBa();
15738 final double[] bias = ba.getBuffer();
15739 final double biasX = ba.getElementAtIndex(0);
15740 final double biasY = ba.getElementAtIndex(1);
15741 final double biasZ = ba.getElementAtIndex(2);
15742
15743 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15744 final double latitude = Math.toRadians(
15745 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15746 final double longitude = Math.toRadians(
15747 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15748 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15749 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15750 final NEDVelocity nedVelocity = new NEDVelocity();
15751 final ECEFPosition ecefPosition = new ECEFPosition();
15752 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15753 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15754 ecefPosition, ecefVelocity);
15755
15756 KnownPositionAccelerometerCalibrator calibrator =
15757 new KnownPositionAccelerometerCalibrator(ecefPosition, bias,
15758 this);
15759
15760
15761 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15762 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15763 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15764 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15765 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15766 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15767 final Acceleration bx2 = new Acceleration(0.0,
15768 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15769 calibrator.getInitialBiasXAsAcceleration(bx2);
15770 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15771 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15772 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15773 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15774 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15775 final Acceleration by2 = new Acceleration(0.0,
15776 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15777 calibrator.getInitialBiasYAsAcceleration(by2);
15778 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15779 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15780 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15781 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15782 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15783 final Acceleration bz2 = new Acceleration(0.0,
15784 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15785 calibrator.getInitialBiasZAsAcceleration(bz2);
15786 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15787 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15788 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
15789 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
15790 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
15791 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
15792 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
15793 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
15794 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
15795 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
15796 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
15797 final double[] bias1 = calibrator.getInitialBias();
15798 assertArrayEquals(bias1, bias, 0.0);
15799 final double[] bias2 = new double[3];
15800 calibrator.getInitialBias(bias2);
15801 assertArrayEquals(bias1, bias2, 0.0);
15802 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15803 assertEquals(b1, ba);
15804 final Matrix b2 = new Matrix(3, 1);
15805 calibrator.getInitialBiasAsMatrix(b2);
15806 assertEquals(b1, b2);
15807 final Matrix ma1 = calibrator.getInitialMa();
15808 assertEquals(ma1, new Matrix(3, 3));
15809 final Matrix ma2 = new Matrix(3, 3);
15810 calibrator.getInitialMa(ma2);
15811 assertEquals(ma1, ma2);
15812 assertSame(calibrator.getEcefPosition(), ecefPosition);
15813 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15814 final NEDPosition nedPosition1 = new NEDPosition();
15815 assertTrue(calibrator.getNedPosition(nedPosition1));
15816 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15817 assertNull(calibrator.getMeasurements());
15818 assertFalse(calibrator.isCommonAxisUsed());
15819 assertSame(calibrator.getListener(), this);
15820 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15821 assertFalse(calibrator.isReady());
15822 assertFalse(calibrator.isRunning());
15823 assertNull(calibrator.getEstimatedBiases());
15824 assertFalse(calibrator.getEstimatedBiases(null));
15825 assertNull(calibrator.getEstimatedBiasesAsMatrix());
15826 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15827 assertNull(calibrator.getEstimatedBiasFx());
15828 assertNull(calibrator.getEstimatedBiasFy());
15829 assertNull(calibrator.getEstimatedBiasFz());
15830 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15831 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15832 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15833 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15834 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15835 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15836 assertNull(calibrator.getEstimatedMa());
15837 assertNull(calibrator.getEstimatedSx());
15838 assertNull(calibrator.getEstimatedSy());
15839 assertNull(calibrator.getEstimatedSz());
15840 assertNull(calibrator.getEstimatedMxy());
15841 assertNull(calibrator.getEstimatedMxz());
15842 assertNull(calibrator.getEstimatedMyx());
15843 assertNull(calibrator.getEstimatedMyz());
15844 assertNull(calibrator.getEstimatedMzx());
15845 assertNull(calibrator.getEstimatedMzy());
15846 assertNull(calibrator.getEstimatedCovariance());
15847 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15848
15849
15850 calibrator = null;
15851 try {
15852 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
15853 new double[1], this);
15854 fail("IllegalArgumentException expected but not thrown");
15855 } catch (final IllegalArgumentException ignore) {
15856 }
15857 assertNull(calibrator);
15858 }
15859
15860 @Test
15861 public void testConstructor135() throws WrongSizeException {
15862 final Collection<StandardDeviationBodyKinematics> measurements =
15863 Collections.emptyList();
15864
15865 final Matrix ba = generateBa();
15866 final double[] bias = ba.getBuffer();
15867 final double biasX = ba.getElementAtIndex(0);
15868 final double biasY = ba.getElementAtIndex(1);
15869 final double biasZ = ba.getElementAtIndex(2);
15870
15871 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15872 final double latitude = Math.toRadians(
15873 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15874 final double longitude = Math.toRadians(
15875 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15876 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15877 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15878 final NEDVelocity nedVelocity = new NEDVelocity();
15879 final ECEFPosition ecefPosition = new ECEFPosition();
15880 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15881 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15882 ecefPosition, ecefVelocity);
15883
15884 KnownPositionAccelerometerCalibrator calibrator =
15885 new KnownPositionAccelerometerCalibrator(ecefPosition,
15886 measurements, bias);
15887
15888
15889 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15890 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15891 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15892 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15893 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15894 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15895 final Acceleration bx2 = new Acceleration(0.0,
15896 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15897 calibrator.getInitialBiasXAsAcceleration(bx2);
15898 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15899 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15900 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15901 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15902 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15903 final Acceleration by2 = new Acceleration(0.0,
15904 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15905 calibrator.getInitialBiasYAsAcceleration(by2);
15906 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15907 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15908 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15909 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15910 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15911 final Acceleration bz2 = new Acceleration(0.0,
15912 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15913 calibrator.getInitialBiasZAsAcceleration(bz2);
15914 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15915 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15916 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
15917 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
15918 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
15919 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
15920 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
15921 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
15922 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
15923 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
15924 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
15925 final double[] bias1 = calibrator.getInitialBias();
15926 assertArrayEquals(bias1, bias, 0.0);
15927 final double[] bias2 = new double[3];
15928 calibrator.getInitialBias(bias2);
15929 assertArrayEquals(bias1, bias2, 0.0);
15930 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15931 assertEquals(b1, ba);
15932 final Matrix b2 = new Matrix(3, 1);
15933 calibrator.getInitialBiasAsMatrix(b2);
15934 assertEquals(b1, b2);
15935 final Matrix ma1 = calibrator.getInitialMa();
15936 assertEquals(ma1, new Matrix(3, 3));
15937 final Matrix ma2 = new Matrix(3, 3);
15938 calibrator.getInitialMa(ma2);
15939 assertEquals(ma1, ma2);
15940 assertSame(calibrator.getEcefPosition(), ecefPosition);
15941 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15942 final NEDPosition nedPosition1 = new NEDPosition();
15943 assertTrue(calibrator.getNedPosition(nedPosition1));
15944 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15945 assertSame(calibrator.getMeasurements(), measurements);
15946 assertFalse(calibrator.isCommonAxisUsed());
15947 assertNull(calibrator.getListener());
15948 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15949 assertFalse(calibrator.isReady());
15950 assertFalse(calibrator.isRunning());
15951 assertNull(calibrator.getEstimatedBiases());
15952 assertFalse(calibrator.getEstimatedBiases(null));
15953 assertNull(calibrator.getEstimatedBiasesAsMatrix());
15954 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15955 assertNull(calibrator.getEstimatedBiasFx());
15956 assertNull(calibrator.getEstimatedBiasFy());
15957 assertNull(calibrator.getEstimatedBiasFz());
15958 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15959 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15960 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15961 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15962 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15963 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15964 assertNull(calibrator.getEstimatedMa());
15965 assertNull(calibrator.getEstimatedSx());
15966 assertNull(calibrator.getEstimatedSy());
15967 assertNull(calibrator.getEstimatedSz());
15968 assertNull(calibrator.getEstimatedMxy());
15969 assertNull(calibrator.getEstimatedMxz());
15970 assertNull(calibrator.getEstimatedMyx());
15971 assertNull(calibrator.getEstimatedMyz());
15972 assertNull(calibrator.getEstimatedMzx());
15973 assertNull(calibrator.getEstimatedMzy());
15974 assertNull(calibrator.getEstimatedCovariance());
15975 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15976
15977
15978 calibrator = null;
15979 try {
15980 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
15981 measurements, new double[1]);
15982 fail("IllegalArgumentException expected but not thrown");
15983 } catch (final IllegalArgumentException ignore) {
15984 }
15985 assertNull(calibrator);
15986 }
15987
15988 @Test
15989 public void testConstructor136() throws WrongSizeException {
15990 final Collection<StandardDeviationBodyKinematics> measurements =
15991 Collections.emptyList();
15992
15993 final Matrix ba = generateBa();
15994 final double[] bias = ba.getBuffer();
15995 final double biasX = ba.getElementAtIndex(0);
15996 final double biasY = ba.getElementAtIndex(1);
15997 final double biasZ = ba.getElementAtIndex(2);
15998
15999 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16000 final double latitude = Math.toRadians(
16001 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16002 final double longitude = Math.toRadians(
16003 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16004 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16005 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16006 final NEDVelocity nedVelocity = new NEDVelocity();
16007 final ECEFPosition ecefPosition = new ECEFPosition();
16008 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16009 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16010 ecefPosition, ecefVelocity);
16011
16012 KnownPositionAccelerometerCalibrator calibrator =
16013 new KnownPositionAccelerometerCalibrator(ecefPosition,
16014 measurements, bias, this);
16015
16016
16017 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16018 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16019 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16020 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16021 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16022 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16023 final Acceleration bx2 = new Acceleration(0.0,
16024 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16025 calibrator.getInitialBiasXAsAcceleration(bx2);
16026 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16027 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16028 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16029 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16030 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16031 final Acceleration by2 = new Acceleration(0.0,
16032 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16033 calibrator.getInitialBiasYAsAcceleration(by2);
16034 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16035 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16036 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16037 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16038 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16039 final Acceleration bz2 = new Acceleration(0.0,
16040 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16041 calibrator.getInitialBiasZAsAcceleration(bz2);
16042 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16043 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16044 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16045 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16046 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16047 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16048 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16049 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16050 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16051 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16052 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16053 final double[] bias1 = calibrator.getInitialBias();
16054 assertArrayEquals(bias1, bias, 0.0);
16055 final double[] bias2 = new double[3];
16056 calibrator.getInitialBias(bias2);
16057 assertArrayEquals(bias1, bias2, 0.0);
16058 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16059 assertEquals(b1, ba);
16060 final Matrix b2 = new Matrix(3, 1);
16061 calibrator.getInitialBiasAsMatrix(b2);
16062 assertEquals(b1, b2);
16063 final Matrix ma1 = calibrator.getInitialMa();
16064 assertEquals(ma1, new Matrix(3, 3));
16065 final Matrix ma2 = new Matrix(3, 3);
16066 calibrator.getInitialMa(ma2);
16067 assertEquals(ma1, ma2);
16068 assertSame(calibrator.getEcefPosition(), ecefPosition);
16069 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16070 final NEDPosition nedPosition1 = new NEDPosition();
16071 assertTrue(calibrator.getNedPosition(nedPosition1));
16072 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16073 assertSame(calibrator.getMeasurements(), measurements);
16074 assertFalse(calibrator.isCommonAxisUsed());
16075 assertSame(calibrator.getListener(), this);
16076 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16077 assertFalse(calibrator.isReady());
16078 assertFalse(calibrator.isRunning());
16079 assertNull(calibrator.getEstimatedBiases());
16080 assertFalse(calibrator.getEstimatedBiases(null));
16081 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16082 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16083 assertNull(calibrator.getEstimatedBiasFx());
16084 assertNull(calibrator.getEstimatedBiasFy());
16085 assertNull(calibrator.getEstimatedBiasFz());
16086 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16087 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16088 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16089 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16090 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16091 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16092 assertNull(calibrator.getEstimatedMa());
16093 assertNull(calibrator.getEstimatedSx());
16094 assertNull(calibrator.getEstimatedSy());
16095 assertNull(calibrator.getEstimatedSz());
16096 assertNull(calibrator.getEstimatedMxy());
16097 assertNull(calibrator.getEstimatedMxz());
16098 assertNull(calibrator.getEstimatedMyx());
16099 assertNull(calibrator.getEstimatedMyz());
16100 assertNull(calibrator.getEstimatedMzx());
16101 assertNull(calibrator.getEstimatedMzy());
16102 assertNull(calibrator.getEstimatedCovariance());
16103 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16104
16105
16106 calibrator = null;
16107 try {
16108 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16109 measurements, new double[1], this);
16110 fail("IllegalArgumentException expected but not thrown");
16111 } catch (final IllegalArgumentException ignore) {
16112 }
16113 assertNull(calibrator);
16114 }
16115
16116 @Test
16117 public void testConstructor137() throws WrongSizeException {
16118 final Matrix ba = generateBa();
16119 final double[] bias = ba.getBuffer();
16120 final double biasX = ba.getElementAtIndex(0);
16121 final double biasY = ba.getElementAtIndex(1);
16122 final double biasZ = ba.getElementAtIndex(2);
16123
16124 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16125 final double latitude = Math.toRadians(
16126 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16127 final double longitude = Math.toRadians(
16128 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16129 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16130 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16131 final NEDVelocity nedVelocity = new NEDVelocity();
16132 final ECEFPosition ecefPosition = new ECEFPosition();
16133 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16134 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16135 ecefPosition, ecefVelocity);
16136
16137 KnownPositionAccelerometerCalibrator calibrator =
16138 new KnownPositionAccelerometerCalibrator(ecefPosition,
16139 true, bias);
16140
16141
16142 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16143 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16144 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16145 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16146 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16147 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16148 final Acceleration bx2 = new Acceleration(0.0,
16149 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16150 calibrator.getInitialBiasXAsAcceleration(bx2);
16151 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16152 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16153 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16154 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16155 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16156 final Acceleration by2 = new Acceleration(0.0,
16157 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16158 calibrator.getInitialBiasYAsAcceleration(by2);
16159 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16160 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16161 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16162 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16163 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16164 final Acceleration bz2 = new Acceleration(0.0,
16165 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16166 calibrator.getInitialBiasZAsAcceleration(bz2);
16167 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16168 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16169 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16170 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16171 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16172 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16173 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16174 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16175 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16176 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16177 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16178 final double[] bias1 = calibrator.getInitialBias();
16179 assertArrayEquals(bias1, bias, 0.0);
16180 final double[] bias2 = new double[3];
16181 calibrator.getInitialBias(bias2);
16182 assertArrayEquals(bias1, bias2, 0.0);
16183 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16184 assertEquals(b1, ba);
16185 final Matrix b2 = new Matrix(3, 1);
16186 calibrator.getInitialBiasAsMatrix(b2);
16187 assertEquals(b1, b2);
16188 final Matrix ma1 = calibrator.getInitialMa();
16189 assertEquals(ma1, new Matrix(3, 3));
16190 final Matrix ma2 = new Matrix(3, 3);
16191 calibrator.getInitialMa(ma2);
16192 assertEquals(ma1, ma2);
16193 assertSame(calibrator.getEcefPosition(), ecefPosition);
16194 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16195 final NEDPosition nedPosition1 = new NEDPosition();
16196 assertTrue(calibrator.getNedPosition(nedPosition1));
16197 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16198 assertNull(calibrator.getMeasurements());
16199 assertTrue(calibrator.isCommonAxisUsed());
16200 assertNull(calibrator.getListener());
16201 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16202 assertFalse(calibrator.isReady());
16203 assertFalse(calibrator.isRunning());
16204 assertNull(calibrator.getEstimatedBiases());
16205 assertFalse(calibrator.getEstimatedBiases(null));
16206 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16207 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16208 assertNull(calibrator.getEstimatedBiasFx());
16209 assertNull(calibrator.getEstimatedBiasFy());
16210 assertNull(calibrator.getEstimatedBiasFz());
16211 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16212 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16213 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16214 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16215 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16216 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16217 assertNull(calibrator.getEstimatedMa());
16218 assertNull(calibrator.getEstimatedSx());
16219 assertNull(calibrator.getEstimatedSy());
16220 assertNull(calibrator.getEstimatedSz());
16221 assertNull(calibrator.getEstimatedMxy());
16222 assertNull(calibrator.getEstimatedMxz());
16223 assertNull(calibrator.getEstimatedMyx());
16224 assertNull(calibrator.getEstimatedMyz());
16225 assertNull(calibrator.getEstimatedMzx());
16226 assertNull(calibrator.getEstimatedMzy());
16227 assertNull(calibrator.getEstimatedCovariance());
16228 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16229
16230
16231 calibrator = null;
16232 try {
16233 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16234 true, new double[1]);
16235 fail("IllegalArgumentException expected but not thrown");
16236 } catch (final IllegalArgumentException ignore) {
16237 }
16238 assertNull(calibrator);
16239 }
16240
16241 @Test
16242 public void testConstructor138() throws WrongSizeException {
16243 final Matrix ba = generateBa();
16244 final double[] bias = ba.getBuffer();
16245 final double biasX = ba.getElementAtIndex(0);
16246 final double biasY = ba.getElementAtIndex(1);
16247 final double biasZ = ba.getElementAtIndex(2);
16248
16249 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16250 final double latitude = Math.toRadians(
16251 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16252 final double longitude = Math.toRadians(
16253 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16254 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16255 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16256 final NEDVelocity nedVelocity = new NEDVelocity();
16257 final ECEFPosition ecefPosition = new ECEFPosition();
16258 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16259 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16260 ecefPosition, ecefVelocity);
16261
16262 KnownPositionAccelerometerCalibrator calibrator =
16263 new KnownPositionAccelerometerCalibrator(ecefPosition,
16264 true, bias, this);
16265
16266
16267 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16268 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16269 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16270 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16271 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16272 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16273 final Acceleration bx2 = new Acceleration(0.0,
16274 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16275 calibrator.getInitialBiasXAsAcceleration(bx2);
16276 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16277 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16278 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16279 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16280 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16281 final Acceleration by2 = new Acceleration(0.0,
16282 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16283 calibrator.getInitialBiasYAsAcceleration(by2);
16284 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16285 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16286 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16287 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16288 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16289 final Acceleration bz2 = new Acceleration(0.0,
16290 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16291 calibrator.getInitialBiasZAsAcceleration(bz2);
16292 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16293 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16294 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16295 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16296 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16297 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16298 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16299 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16300 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16301 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16302 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16303 final double[] bias1 = calibrator.getInitialBias();
16304 assertArrayEquals(bias1, bias, 0.0);
16305 final double[] bias2 = new double[3];
16306 calibrator.getInitialBias(bias2);
16307 assertArrayEquals(bias1, bias2, 0.0);
16308 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16309 assertEquals(b1, ba);
16310 final Matrix b2 = new Matrix(3, 1);
16311 calibrator.getInitialBiasAsMatrix(b2);
16312 assertEquals(b1, b2);
16313 final Matrix ma1 = calibrator.getInitialMa();
16314 assertEquals(ma1, new Matrix(3, 3));
16315 final Matrix ma2 = new Matrix(3, 3);
16316 calibrator.getInitialMa(ma2);
16317 assertEquals(ma1, ma2);
16318 assertSame(calibrator.getEcefPosition(), ecefPosition);
16319 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16320 final NEDPosition nedPosition1 = new NEDPosition();
16321 assertTrue(calibrator.getNedPosition(nedPosition1));
16322 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16323 assertNull(calibrator.getMeasurements());
16324 assertTrue(calibrator.isCommonAxisUsed());
16325 assertSame(calibrator.getListener(), this);
16326 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16327 assertFalse(calibrator.isReady());
16328 assertFalse(calibrator.isRunning());
16329 assertNull(calibrator.getEstimatedBiases());
16330 assertFalse(calibrator.getEstimatedBiases(null));
16331 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16332 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16333 assertNull(calibrator.getEstimatedBiasFx());
16334 assertNull(calibrator.getEstimatedBiasFy());
16335 assertNull(calibrator.getEstimatedBiasFz());
16336 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16337 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16338 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16339 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16340 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16341 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16342 assertNull(calibrator.getEstimatedMa());
16343 assertNull(calibrator.getEstimatedSx());
16344 assertNull(calibrator.getEstimatedSy());
16345 assertNull(calibrator.getEstimatedSz());
16346 assertNull(calibrator.getEstimatedMxy());
16347 assertNull(calibrator.getEstimatedMxz());
16348 assertNull(calibrator.getEstimatedMyx());
16349 assertNull(calibrator.getEstimatedMyz());
16350 assertNull(calibrator.getEstimatedMzx());
16351 assertNull(calibrator.getEstimatedMzy());
16352 assertNull(calibrator.getEstimatedCovariance());
16353 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16354
16355
16356 calibrator = null;
16357 try {
16358 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16359 true, new double[1], this);
16360 fail("IllegalArgumentException expected but not thrown");
16361 } catch (final IllegalArgumentException ignore) {
16362 }
16363 assertNull(calibrator);
16364 }
16365
16366 @Test
16367 public void testConstructor139() throws WrongSizeException {
16368 final Collection<StandardDeviationBodyKinematics> measurements =
16369 Collections.emptyList();
16370
16371 final Matrix ba = generateBa();
16372 final double[] bias = ba.getBuffer();
16373 final double biasX = ba.getElementAtIndex(0);
16374 final double biasY = ba.getElementAtIndex(1);
16375 final double biasZ = ba.getElementAtIndex(2);
16376
16377 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16378 final double latitude = Math.toRadians(
16379 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16380 final double longitude = Math.toRadians(
16381 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16382 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16383 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16384 final NEDVelocity nedVelocity = new NEDVelocity();
16385 final ECEFPosition ecefPosition = new ECEFPosition();
16386 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16387 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16388 ecefPosition, ecefVelocity);
16389
16390 KnownPositionAccelerometerCalibrator calibrator =
16391 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
16392 true, bias);
16393
16394
16395 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16396 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16397 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16398 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16399 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16400 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16401 final Acceleration bx2 = new Acceleration(0.0,
16402 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16403 calibrator.getInitialBiasXAsAcceleration(bx2);
16404 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16405 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16406 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16407 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16408 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16409 final Acceleration by2 = new Acceleration(0.0,
16410 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16411 calibrator.getInitialBiasYAsAcceleration(by2);
16412 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16413 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16414 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16415 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16416 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16417 final Acceleration bz2 = new Acceleration(0.0,
16418 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16419 calibrator.getInitialBiasZAsAcceleration(bz2);
16420 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16421 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16422 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16423 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16424 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16425 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16426 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16427 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16428 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16429 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16430 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16431 final double[] bias1 = calibrator.getInitialBias();
16432 assertArrayEquals(bias1, bias, 0.0);
16433 final double[] bias2 = new double[3];
16434 calibrator.getInitialBias(bias2);
16435 assertArrayEquals(bias1, bias2, 0.0);
16436 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16437 assertEquals(b1, ba);
16438 final Matrix b2 = new Matrix(3, 1);
16439 calibrator.getInitialBiasAsMatrix(b2);
16440 assertEquals(b1, b2);
16441 final Matrix ma1 = calibrator.getInitialMa();
16442 assertEquals(ma1, new Matrix(3, 3));
16443 final Matrix ma2 = new Matrix(3, 3);
16444 calibrator.getInitialMa(ma2);
16445 assertEquals(ma1, ma2);
16446 assertSame(calibrator.getEcefPosition(), ecefPosition);
16447 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16448 final NEDPosition nedPosition1 = new NEDPosition();
16449 assertTrue(calibrator.getNedPosition(nedPosition1));
16450 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16451 assertSame(calibrator.getMeasurements(), measurements);
16452 assertTrue(calibrator.isCommonAxisUsed());
16453 assertNull(calibrator.getListener());
16454 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16455 assertFalse(calibrator.isReady());
16456 assertFalse(calibrator.isRunning());
16457 assertNull(calibrator.getEstimatedBiases());
16458 assertFalse(calibrator.getEstimatedBiases(null));
16459 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16460 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16461 assertNull(calibrator.getEstimatedBiasFx());
16462 assertNull(calibrator.getEstimatedBiasFy());
16463 assertNull(calibrator.getEstimatedBiasFz());
16464 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16465 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16466 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16467 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16468 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16469 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16470 assertNull(calibrator.getEstimatedMa());
16471 assertNull(calibrator.getEstimatedSx());
16472 assertNull(calibrator.getEstimatedSy());
16473 assertNull(calibrator.getEstimatedSz());
16474 assertNull(calibrator.getEstimatedMxy());
16475 assertNull(calibrator.getEstimatedMxz());
16476 assertNull(calibrator.getEstimatedMyx());
16477 assertNull(calibrator.getEstimatedMyz());
16478 assertNull(calibrator.getEstimatedMzx());
16479 assertNull(calibrator.getEstimatedMzy());
16480 assertNull(calibrator.getEstimatedCovariance());
16481 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16482
16483
16484 calibrator = null;
16485 try {
16486 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16487 measurements, true, new double[1]);
16488 fail("IllegalArgumentException expected but not thrown");
16489 } catch (final IllegalArgumentException ignore) {
16490 }
16491 assertNull(calibrator);
16492 }
16493
16494 @Test
16495 public void testConstructor140() throws WrongSizeException {
16496 final Collection<StandardDeviationBodyKinematics> measurements =
16497 Collections.emptyList();
16498
16499 final Matrix ba = generateBa();
16500 final double[] bias = ba.getBuffer();
16501 final double biasX = ba.getElementAtIndex(0);
16502 final double biasY = ba.getElementAtIndex(1);
16503 final double biasZ = ba.getElementAtIndex(2);
16504
16505 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16506 final double latitude = Math.toRadians(
16507 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16508 final double longitude = Math.toRadians(
16509 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16510 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16511 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16512 final NEDVelocity nedVelocity = new NEDVelocity();
16513 final ECEFPosition ecefPosition = new ECEFPosition();
16514 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16515 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16516 ecefPosition, ecefVelocity);
16517
16518 KnownPositionAccelerometerCalibrator calibrator =
16519 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
16520 true, bias, this);
16521
16522
16523 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16524 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16525 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16526 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16527 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16528 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16529 final Acceleration bx2 = new Acceleration(0.0,
16530 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16531 calibrator.getInitialBiasXAsAcceleration(bx2);
16532 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16533 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16534 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16535 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16536 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16537 final Acceleration by2 = new Acceleration(0.0,
16538 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16539 calibrator.getInitialBiasYAsAcceleration(by2);
16540 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16541 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16542 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16543 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16544 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16545 final Acceleration bz2 = new Acceleration(0.0,
16546 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16547 calibrator.getInitialBiasZAsAcceleration(bz2);
16548 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16549 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16550 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16551 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16552 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16553 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16554 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16555 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16556 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16557 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16558 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16559 final double[] bias1 = calibrator.getInitialBias();
16560 assertArrayEquals(bias1, bias, 0.0);
16561 final double[] bias2 = new double[3];
16562 calibrator.getInitialBias(bias2);
16563 assertArrayEquals(bias1, bias2, 0.0);
16564 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16565 assertEquals(b1, ba);
16566 final Matrix b2 = new Matrix(3, 1);
16567 calibrator.getInitialBiasAsMatrix(b2);
16568 assertEquals(b1, b2);
16569 final Matrix ma1 = calibrator.getInitialMa();
16570 assertEquals(ma1, new Matrix(3, 3));
16571 final Matrix ma2 = new Matrix(3, 3);
16572 calibrator.getInitialMa(ma2);
16573 assertEquals(ma1, ma2);
16574 assertSame(calibrator.getEcefPosition(), ecefPosition);
16575 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16576 final NEDPosition nedPosition1 = new NEDPosition();
16577 assertTrue(calibrator.getNedPosition(nedPosition1));
16578 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16579 assertSame(calibrator.getMeasurements(), measurements);
16580 assertTrue(calibrator.isCommonAxisUsed());
16581 assertSame(calibrator.getListener(), this);
16582 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16583 assertFalse(calibrator.isReady());
16584 assertFalse(calibrator.isRunning());
16585 assertNull(calibrator.getEstimatedBiases());
16586 assertFalse(calibrator.getEstimatedBiases(null));
16587 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16588 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16589 assertNull(calibrator.getEstimatedBiasFx());
16590 assertNull(calibrator.getEstimatedBiasFy());
16591 assertNull(calibrator.getEstimatedBiasFz());
16592 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16593 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16594 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16595 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16596 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16597 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16598 assertNull(calibrator.getEstimatedMa());
16599 assertNull(calibrator.getEstimatedSx());
16600 assertNull(calibrator.getEstimatedSy());
16601 assertNull(calibrator.getEstimatedSz());
16602 assertNull(calibrator.getEstimatedMxy());
16603 assertNull(calibrator.getEstimatedMxz());
16604 assertNull(calibrator.getEstimatedMyx());
16605 assertNull(calibrator.getEstimatedMyz());
16606 assertNull(calibrator.getEstimatedMzx());
16607 assertNull(calibrator.getEstimatedMzy());
16608 assertNull(calibrator.getEstimatedCovariance());
16609 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16610
16611
16612 calibrator = null;
16613 try {
16614 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16615 measurements, true, new double[1], this);
16616 fail("IllegalArgumentException expected but not thrown");
16617 } catch (final IllegalArgumentException ignore) {
16618 }
16619 assertNull(calibrator);
16620 }
16621
16622 @Test
16623 public void testConstructor141() throws WrongSizeException {
16624 final Matrix ba = generateBa();
16625 final double[] bias = ba.getBuffer();
16626 final double biasX = ba.getElementAtIndex(0);
16627 final double biasY = ba.getElementAtIndex(1);
16628 final double biasZ = ba.getElementAtIndex(2);
16629
16630 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16631 final double latitude = Math.toRadians(
16632 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16633 final double longitude = Math.toRadians(
16634 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16635 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16636 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16637 final NEDVelocity nedVelocity = new NEDVelocity();
16638 final ECEFPosition ecefPosition = new ECEFPosition();
16639 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16640 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16641 ecefPosition, ecefVelocity);
16642
16643 KnownPositionAccelerometerCalibrator calibrator =
16644 new KnownPositionAccelerometerCalibrator(ecefPosition, ba);
16645
16646
16647 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16648 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16649 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16650 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16651 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16652 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16653 final Acceleration bx2 = new Acceleration(0.0,
16654 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16655 calibrator.getInitialBiasXAsAcceleration(bx2);
16656 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16657 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16658 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16659 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16660 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16661 final Acceleration by2 = new Acceleration(0.0,
16662 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16663 calibrator.getInitialBiasYAsAcceleration(by2);
16664 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16665 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16666 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16667 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16668 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16669 final Acceleration bz2 = new Acceleration(0.0,
16670 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16671 calibrator.getInitialBiasZAsAcceleration(bz2);
16672 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16673 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16674 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16675 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16676 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16677 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16678 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16679 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16680 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16681 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16682 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16683 final double[] bias1 = calibrator.getInitialBias();
16684 assertArrayEquals(bias1, bias, 0.0);
16685 final double[] bias2 = new double[3];
16686 calibrator.getInitialBias(bias2);
16687 assertArrayEquals(bias1, bias2, 0.0);
16688 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16689 assertEquals(b1, ba);
16690 final Matrix b2 = new Matrix(3, 1);
16691 calibrator.getInitialBiasAsMatrix(b2);
16692 assertEquals(b1, b2);
16693 final Matrix ma1 = calibrator.getInitialMa();
16694 assertEquals(ma1, new Matrix(3, 3));
16695 final Matrix ma2 = new Matrix(3, 3);
16696 calibrator.getInitialMa(ma2);
16697 assertEquals(ma1, ma2);
16698 assertSame(calibrator.getEcefPosition(), ecefPosition);
16699 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16700 final NEDPosition nedPosition1 = new NEDPosition();
16701 assertTrue(calibrator.getNedPosition(nedPosition1));
16702 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16703 assertNull(calibrator.getMeasurements());
16704 assertFalse(calibrator.isCommonAxisUsed());
16705 assertNull(calibrator.getListener());
16706 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16707 assertFalse(calibrator.isReady());
16708 assertFalse(calibrator.isRunning());
16709 assertNull(calibrator.getEstimatedBiases());
16710 assertFalse(calibrator.getEstimatedBiases(null));
16711 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16712 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16713 assertNull(calibrator.getEstimatedBiasFx());
16714 assertNull(calibrator.getEstimatedBiasFy());
16715 assertNull(calibrator.getEstimatedBiasFz());
16716 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16717 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16718 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16719 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16720 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16721 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16722 assertNull(calibrator.getEstimatedMa());
16723 assertNull(calibrator.getEstimatedSx());
16724 assertNull(calibrator.getEstimatedSy());
16725 assertNull(calibrator.getEstimatedSz());
16726 assertNull(calibrator.getEstimatedMxy());
16727 assertNull(calibrator.getEstimatedMxz());
16728 assertNull(calibrator.getEstimatedMyx());
16729 assertNull(calibrator.getEstimatedMyz());
16730 assertNull(calibrator.getEstimatedMzx());
16731 assertNull(calibrator.getEstimatedMzy());
16732 assertNull(calibrator.getEstimatedCovariance());
16733 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16734
16735
16736 calibrator = null;
16737 try {
16738 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16739 new Matrix(1, 1));
16740 fail("IllegalArgumentException expected but not thrown");
16741 } catch (final IllegalArgumentException ignore) {
16742 }
16743 try {
16744 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16745 new Matrix(1, 3));
16746 fail("IllegalArgumentException expected but not thrown");
16747 } catch (final IllegalArgumentException ignore) {
16748 }
16749 assertNull(calibrator);
16750 }
16751
16752 @Test
16753 public void testConstructor142() throws WrongSizeException {
16754 final Matrix ba = generateBa();
16755 final double[] bias = ba.getBuffer();
16756 final double biasX = ba.getElementAtIndex(0);
16757 final double biasY = ba.getElementAtIndex(1);
16758 final double biasZ = ba.getElementAtIndex(2);
16759
16760 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16761 final double latitude = Math.toRadians(
16762 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16763 final double longitude = Math.toRadians(
16764 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16765 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16766 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16767 final NEDVelocity nedVelocity = new NEDVelocity();
16768 final ECEFPosition ecefPosition = new ECEFPosition();
16769 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16770 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16771 ecefPosition, ecefVelocity);
16772
16773 KnownPositionAccelerometerCalibrator calibrator =
16774 new KnownPositionAccelerometerCalibrator(ecefPosition, ba,
16775 this);
16776
16777
16778 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16779 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16780 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16781 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16782 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16783 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16784 final Acceleration bx2 = new Acceleration(0.0,
16785 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16786 calibrator.getInitialBiasXAsAcceleration(bx2);
16787 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16788 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16789 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16790 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16791 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16792 final Acceleration by2 = new Acceleration(0.0,
16793 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16794 calibrator.getInitialBiasYAsAcceleration(by2);
16795 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16796 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16797 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16798 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16799 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16800 final Acceleration bz2 = new Acceleration(0.0,
16801 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16802 calibrator.getInitialBiasZAsAcceleration(bz2);
16803 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16804 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16805 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16806 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16807 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16808 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16809 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16810 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16811 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16812 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16813 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16814 final double[] bias1 = calibrator.getInitialBias();
16815 assertArrayEquals(bias1, bias, 0.0);
16816 final double[] bias2 = new double[3];
16817 calibrator.getInitialBias(bias2);
16818 assertArrayEquals(bias1, bias2, 0.0);
16819 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16820 assertEquals(b1, ba);
16821 final Matrix b2 = new Matrix(3, 1);
16822 calibrator.getInitialBiasAsMatrix(b2);
16823 assertEquals(b1, b2);
16824 final Matrix ma1 = calibrator.getInitialMa();
16825 assertEquals(ma1, new Matrix(3, 3));
16826 final Matrix ma2 = new Matrix(3, 3);
16827 calibrator.getInitialMa(ma2);
16828 assertEquals(ma1, ma2);
16829 assertSame(calibrator.getEcefPosition(), ecefPosition);
16830 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16831 final NEDPosition nedPosition1 = new NEDPosition();
16832 assertTrue(calibrator.getNedPosition(nedPosition1));
16833 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16834 assertNull(calibrator.getMeasurements());
16835 assertFalse(calibrator.isCommonAxisUsed());
16836 assertSame(calibrator.getListener(), this);
16837 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16838 assertFalse(calibrator.isReady());
16839 assertFalse(calibrator.isRunning());
16840 assertNull(calibrator.getEstimatedBiases());
16841 assertFalse(calibrator.getEstimatedBiases(null));
16842 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16843 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16844 assertNull(calibrator.getEstimatedBiasFx());
16845 assertNull(calibrator.getEstimatedBiasFy());
16846 assertNull(calibrator.getEstimatedBiasFz());
16847 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16848 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16849 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16850 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16851 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16852 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16853 assertNull(calibrator.getEstimatedMa());
16854 assertNull(calibrator.getEstimatedSx());
16855 assertNull(calibrator.getEstimatedSy());
16856 assertNull(calibrator.getEstimatedSz());
16857 assertNull(calibrator.getEstimatedMxy());
16858 assertNull(calibrator.getEstimatedMxz());
16859 assertNull(calibrator.getEstimatedMyx());
16860 assertNull(calibrator.getEstimatedMyz());
16861 assertNull(calibrator.getEstimatedMzx());
16862 assertNull(calibrator.getEstimatedMzy());
16863 assertNull(calibrator.getEstimatedCovariance());
16864 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16865
16866
16867 calibrator = null;
16868 try {
16869 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16870 new Matrix(1, 1), this);
16871 fail("IllegalArgumentException expected but not thrown");
16872 } catch (final IllegalArgumentException ignore) {
16873 }
16874 try {
16875 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16876 new Matrix(1, 3), this);
16877 fail("IllegalArgumentException expected but not thrown");
16878 } catch (final IllegalArgumentException ignore) {
16879 }
16880 assertNull(calibrator);
16881 }
16882
16883 @Test
16884 public void testConstructor143() throws WrongSizeException {
16885 final Collection<StandardDeviationBodyKinematics> measurements =
16886 Collections.emptyList();
16887
16888 final Matrix ba = generateBa();
16889 final double[] bias = ba.getBuffer();
16890 final double biasX = ba.getElementAtIndex(0);
16891 final double biasY = ba.getElementAtIndex(1);
16892 final double biasZ = ba.getElementAtIndex(2);
16893
16894 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16895 final double latitude = Math.toRadians(
16896 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16897 final double longitude = Math.toRadians(
16898 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16899 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16900 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16901 final NEDVelocity nedVelocity = new NEDVelocity();
16902 final ECEFPosition ecefPosition = new ECEFPosition();
16903 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16904 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16905 ecefPosition, ecefVelocity);
16906
16907 KnownPositionAccelerometerCalibrator calibrator =
16908 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
16909 ba);
16910
16911
16912 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16913 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16914 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16915 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16916 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16917 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16918 final Acceleration bx2 = new Acceleration(0.0,
16919 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16920 calibrator.getInitialBiasXAsAcceleration(bx2);
16921 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16922 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16923 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16924 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16925 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16926 final Acceleration by2 = new Acceleration(0.0,
16927 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16928 calibrator.getInitialBiasYAsAcceleration(by2);
16929 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16930 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16931 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16932 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16933 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16934 final Acceleration bz2 = new Acceleration(0.0,
16935 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16936 calibrator.getInitialBiasZAsAcceleration(bz2);
16937 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16938 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16939 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16940 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16941 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16942 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16943 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16944 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16945 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16946 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16947 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16948 final double[] bias1 = calibrator.getInitialBias();
16949 assertArrayEquals(bias1, bias, 0.0);
16950 final double[] bias2 = new double[3];
16951 calibrator.getInitialBias(bias2);
16952 assertArrayEquals(bias1, bias2, 0.0);
16953 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16954 assertEquals(b1, ba);
16955 final Matrix b2 = new Matrix(3, 1);
16956 calibrator.getInitialBiasAsMatrix(b2);
16957 assertEquals(b1, b2);
16958 final Matrix ma1 = calibrator.getInitialMa();
16959 assertEquals(ma1, new Matrix(3, 3));
16960 final Matrix ma2 = new Matrix(3, 3);
16961 calibrator.getInitialMa(ma2);
16962 assertEquals(ma1, ma2);
16963 assertSame(calibrator.getEcefPosition(), ecefPosition);
16964 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16965 final NEDPosition nedPosition1 = new NEDPosition();
16966 assertTrue(calibrator.getNedPosition(nedPosition1));
16967 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16968 assertSame(calibrator.getMeasurements(), measurements);
16969 assertFalse(calibrator.isCommonAxisUsed());
16970 assertNull(calibrator.getListener());
16971 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16972 assertFalse(calibrator.isReady());
16973 assertFalse(calibrator.isRunning());
16974 assertNull(calibrator.getEstimatedBiases());
16975 assertFalse(calibrator.getEstimatedBiases(null));
16976 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16977 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16978 assertNull(calibrator.getEstimatedBiasFx());
16979 assertNull(calibrator.getEstimatedBiasFy());
16980 assertNull(calibrator.getEstimatedBiasFz());
16981 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16982 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16983 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16984 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16985 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16986 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16987 assertNull(calibrator.getEstimatedMa());
16988 assertNull(calibrator.getEstimatedSx());
16989 assertNull(calibrator.getEstimatedSy());
16990 assertNull(calibrator.getEstimatedSz());
16991 assertNull(calibrator.getEstimatedMxy());
16992 assertNull(calibrator.getEstimatedMxz());
16993 assertNull(calibrator.getEstimatedMyx());
16994 assertNull(calibrator.getEstimatedMyz());
16995 assertNull(calibrator.getEstimatedMzx());
16996 assertNull(calibrator.getEstimatedMzy());
16997 assertNull(calibrator.getEstimatedCovariance());
16998 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16999
17000
17001 calibrator = null;
17002 try {
17003 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17004 measurements, new Matrix(1, 1));
17005 fail("IllegalArgumentException expected but not thrown");
17006 } catch (final IllegalArgumentException ignore) {
17007 }
17008 try {
17009 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17010 measurements, new Matrix(1, 3));
17011 fail("IllegalArgumentException expected but not thrown");
17012 } catch (final IllegalArgumentException ignore) {
17013 }
17014 assertNull(calibrator);
17015 }
17016
17017 @Test
17018 public void testConstructor144() throws WrongSizeException {
17019 final Collection<StandardDeviationBodyKinematics> measurements =
17020 Collections.emptyList();
17021
17022 final Matrix ba = generateBa();
17023 final double[] bias = ba.getBuffer();
17024 final double biasX = ba.getElementAtIndex(0);
17025 final double biasY = ba.getElementAtIndex(1);
17026 final double biasZ = ba.getElementAtIndex(2);
17027
17028 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17029 final double latitude = Math.toRadians(
17030 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17031 final double longitude = Math.toRadians(
17032 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17033 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17034 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17035 final NEDVelocity nedVelocity = new NEDVelocity();
17036 final ECEFPosition ecefPosition = new ECEFPosition();
17037 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17038 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17039 ecefPosition, ecefVelocity);
17040
17041 KnownPositionAccelerometerCalibrator calibrator =
17042 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
17043 ba, this);
17044
17045
17046 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17047 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17048 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17049 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17050 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17051 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17052 final Acceleration bx2 = new Acceleration(0.0,
17053 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17054 calibrator.getInitialBiasXAsAcceleration(bx2);
17055 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17056 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17057 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17058 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17059 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17060 final Acceleration by2 = new Acceleration(0.0,
17061 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17062 calibrator.getInitialBiasYAsAcceleration(by2);
17063 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17064 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17065 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17066 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17067 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17068 final Acceleration bz2 = new Acceleration(0.0,
17069 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17070 calibrator.getInitialBiasZAsAcceleration(bz2);
17071 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17072 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17073 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17074 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17075 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17076 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17077 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17078 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17079 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17080 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17081 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17082 final double[] bias1 = calibrator.getInitialBias();
17083 assertArrayEquals(bias1, bias, 0.0);
17084 final double[] bias2 = new double[3];
17085 calibrator.getInitialBias(bias2);
17086 assertArrayEquals(bias1, bias2, 0.0);
17087 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17088 assertEquals(b1, ba);
17089 final Matrix b2 = new Matrix(3, 1);
17090 calibrator.getInitialBiasAsMatrix(b2);
17091 assertEquals(b1, b2);
17092 final Matrix ma1 = calibrator.getInitialMa();
17093 assertEquals(ma1, new Matrix(3, 3));
17094 final Matrix ma2 = new Matrix(3, 3);
17095 calibrator.getInitialMa(ma2);
17096 assertEquals(ma1, ma2);
17097 assertSame(calibrator.getEcefPosition(), ecefPosition);
17098 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17099 final NEDPosition nedPosition1 = new NEDPosition();
17100 assertTrue(calibrator.getNedPosition(nedPosition1));
17101 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17102 assertSame(calibrator.getMeasurements(), measurements);
17103 assertFalse(calibrator.isCommonAxisUsed());
17104 assertSame(calibrator.getListener(), this);
17105 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
17106 assertFalse(calibrator.isReady());
17107 assertFalse(calibrator.isRunning());
17108 assertNull(calibrator.getEstimatedBiases());
17109 assertFalse(calibrator.getEstimatedBiases(null));
17110 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17111 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17112 assertNull(calibrator.getEstimatedBiasFx());
17113 assertNull(calibrator.getEstimatedBiasFy());
17114 assertNull(calibrator.getEstimatedBiasFz());
17115 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17116 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17117 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17118 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17119 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17120 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17121 assertNull(calibrator.getEstimatedMa());
17122 assertNull(calibrator.getEstimatedSx());
17123 assertNull(calibrator.getEstimatedSy());
17124 assertNull(calibrator.getEstimatedSz());
17125 assertNull(calibrator.getEstimatedMxy());
17126 assertNull(calibrator.getEstimatedMxz());
17127 assertNull(calibrator.getEstimatedMyx());
17128 assertNull(calibrator.getEstimatedMyz());
17129 assertNull(calibrator.getEstimatedMzx());
17130 assertNull(calibrator.getEstimatedMzy());
17131 assertNull(calibrator.getEstimatedCovariance());
17132 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17133
17134
17135 calibrator = null;
17136 try {
17137 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17138 measurements, new Matrix(1, 1), this);
17139 fail("IllegalArgumentException expected but not thrown");
17140 } catch (final IllegalArgumentException ignore) {
17141 }
17142 try {
17143 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17144 measurements, new Matrix(1, 3), this);
17145 fail("IllegalArgumentException expected but not thrown");
17146 } catch (final IllegalArgumentException ignore) {
17147 }
17148 assertNull(calibrator);
17149 }
17150
17151 @Test
17152 public void testConstructor145() throws WrongSizeException {
17153 final Matrix ba = generateBa();
17154 final double[] bias = ba.getBuffer();
17155 final double biasX = ba.getElementAtIndex(0);
17156 final double biasY = ba.getElementAtIndex(1);
17157 final double biasZ = ba.getElementAtIndex(2);
17158
17159 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17160 final double latitude = Math.toRadians(
17161 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17162 final double longitude = Math.toRadians(
17163 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17164 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17165 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17166 final NEDVelocity nedVelocity = new NEDVelocity();
17167 final ECEFPosition ecefPosition = new ECEFPosition();
17168 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17169 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17170 ecefPosition, ecefVelocity);
17171
17172 KnownPositionAccelerometerCalibrator calibrator =
17173 new KnownPositionAccelerometerCalibrator(ecefPosition,
17174 true, ba);
17175
17176
17177 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17178 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17179 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17180 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17181 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17182 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17183 final Acceleration bx2 = new Acceleration(0.0,
17184 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17185 calibrator.getInitialBiasXAsAcceleration(bx2);
17186 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17187 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17188 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17189 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17190 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17191 final Acceleration by2 = new Acceleration(0.0,
17192 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17193 calibrator.getInitialBiasYAsAcceleration(by2);
17194 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17195 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17196 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17197 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17198 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17199 final Acceleration bz2 = new Acceleration(0.0,
17200 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17201 calibrator.getInitialBiasZAsAcceleration(bz2);
17202 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17203 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17204 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17205 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17206 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17207 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17208 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17209 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17210 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17211 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17212 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17213 final double[] bias1 = calibrator.getInitialBias();
17214 assertArrayEquals(bias1, bias, 0.0);
17215 final double[] bias2 = new double[3];
17216 calibrator.getInitialBias(bias2);
17217 assertArrayEquals(bias1, bias2, 0.0);
17218 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17219 assertEquals(b1, ba);
17220 final Matrix b2 = new Matrix(3, 1);
17221 calibrator.getInitialBiasAsMatrix(b2);
17222 assertEquals(b1, b2);
17223 final Matrix ma1 = calibrator.getInitialMa();
17224 assertEquals(ma1, new Matrix(3, 3));
17225 final Matrix ma2 = new Matrix(3, 3);
17226 calibrator.getInitialMa(ma2);
17227 assertEquals(ma1, ma2);
17228 assertSame(calibrator.getEcefPosition(), ecefPosition);
17229 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17230 final NEDPosition nedPosition1 = new NEDPosition();
17231 assertTrue(calibrator.getNedPosition(nedPosition1));
17232 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17233 assertNull(calibrator.getMeasurements());
17234 assertTrue(calibrator.isCommonAxisUsed());
17235 assertNull(calibrator.getListener());
17236 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17237 assertFalse(calibrator.isReady());
17238 assertFalse(calibrator.isRunning());
17239 assertNull(calibrator.getEstimatedBiases());
17240 assertFalse(calibrator.getEstimatedBiases(null));
17241 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17242 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17243 assertNull(calibrator.getEstimatedBiasFx());
17244 assertNull(calibrator.getEstimatedBiasFy());
17245 assertNull(calibrator.getEstimatedBiasFz());
17246 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17247 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17248 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17249 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17250 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17251 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17252 assertNull(calibrator.getEstimatedMa());
17253 assertNull(calibrator.getEstimatedSx());
17254 assertNull(calibrator.getEstimatedSy());
17255 assertNull(calibrator.getEstimatedSz());
17256 assertNull(calibrator.getEstimatedMxy());
17257 assertNull(calibrator.getEstimatedMxz());
17258 assertNull(calibrator.getEstimatedMyx());
17259 assertNull(calibrator.getEstimatedMyz());
17260 assertNull(calibrator.getEstimatedMzx());
17261 assertNull(calibrator.getEstimatedMzy());
17262 assertNull(calibrator.getEstimatedCovariance());
17263 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17264
17265
17266 calibrator = null;
17267 try {
17268 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17269 true, new Matrix(1, 1));
17270 fail("IllegalArgumentException expected but not thrown");
17271 } catch (final IllegalArgumentException ignore) {
17272 }
17273 try {
17274 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17275 true, new Matrix(1, 3));
17276 fail("IllegalArgumentException expected but not thrown");
17277 } catch (final IllegalArgumentException ignore) {
17278 }
17279 assertNull(calibrator);
17280 }
17281
17282 @Test
17283 public void testConstructor146() throws WrongSizeException {
17284 final Matrix ba = generateBa();
17285 final double[] bias = ba.getBuffer();
17286 final double biasX = ba.getElementAtIndex(0);
17287 final double biasY = ba.getElementAtIndex(1);
17288 final double biasZ = ba.getElementAtIndex(2);
17289
17290 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17291 final double latitude = Math.toRadians(
17292 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17293 final double longitude = Math.toRadians(
17294 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17295 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17296 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17297 final NEDVelocity nedVelocity = new NEDVelocity();
17298 final ECEFPosition ecefPosition = new ECEFPosition();
17299 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17300 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17301 ecefPosition, ecefVelocity);
17302
17303 KnownPositionAccelerometerCalibrator calibrator =
17304 new KnownPositionAccelerometerCalibrator(ecefPosition,
17305 true, ba, this);
17306
17307
17308 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17309 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17310 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17311 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17312 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17313 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17314 final Acceleration bx2 = new Acceleration(0.0,
17315 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17316 calibrator.getInitialBiasXAsAcceleration(bx2);
17317 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17318 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17319 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17320 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17321 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17322 final Acceleration by2 = new Acceleration(0.0,
17323 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17324 calibrator.getInitialBiasYAsAcceleration(by2);
17325 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17326 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17327 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17328 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17329 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17330 final Acceleration bz2 = new Acceleration(0.0,
17331 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17332 calibrator.getInitialBiasZAsAcceleration(bz2);
17333 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17334 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17335 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17336 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17337 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17338 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17339 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17340 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17341 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17342 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17343 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17344 final double[] bias1 = calibrator.getInitialBias();
17345 assertArrayEquals(bias1, bias, 0.0);
17346 final double[] bias2 = new double[3];
17347 calibrator.getInitialBias(bias2);
17348 assertArrayEquals(bias1, bias2, 0.0);
17349 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17350 assertEquals(b1, ba);
17351 final Matrix b2 = new Matrix(3, 1);
17352 calibrator.getInitialBiasAsMatrix(b2);
17353 assertEquals(b1, b2);
17354 final Matrix ma1 = calibrator.getInitialMa();
17355 assertEquals(ma1, new Matrix(3, 3));
17356 final Matrix ma2 = new Matrix(3, 3);
17357 calibrator.getInitialMa(ma2);
17358 assertEquals(ma1, ma2);
17359 assertSame(calibrator.getEcefPosition(), ecefPosition);
17360 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17361 final NEDPosition nedPosition1 = new NEDPosition();
17362 assertTrue(calibrator.getNedPosition(nedPosition1));
17363 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17364 assertNull(calibrator.getMeasurements());
17365 assertTrue(calibrator.isCommonAxisUsed());
17366 assertSame(calibrator.getListener(), this);
17367 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17368 assertFalse(calibrator.isReady());
17369 assertFalse(calibrator.isRunning());
17370 assertNull(calibrator.getEstimatedBiases());
17371 assertFalse(calibrator.getEstimatedBiases(null));
17372 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17373 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17374 assertNull(calibrator.getEstimatedBiasFx());
17375 assertNull(calibrator.getEstimatedBiasFy());
17376 assertNull(calibrator.getEstimatedBiasFz());
17377 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17378 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17379 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17380 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17381 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17382 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17383 assertNull(calibrator.getEstimatedMa());
17384 assertNull(calibrator.getEstimatedSx());
17385 assertNull(calibrator.getEstimatedSy());
17386 assertNull(calibrator.getEstimatedSz());
17387 assertNull(calibrator.getEstimatedMxy());
17388 assertNull(calibrator.getEstimatedMxz());
17389 assertNull(calibrator.getEstimatedMyx());
17390 assertNull(calibrator.getEstimatedMyz());
17391 assertNull(calibrator.getEstimatedMzx());
17392 assertNull(calibrator.getEstimatedMzy());
17393 assertNull(calibrator.getEstimatedCovariance());
17394 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17395
17396
17397 calibrator = null;
17398 try {
17399 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17400 true, new Matrix(1, 1),
17401 this);
17402 fail("IllegalArgumentException expected but not thrown");
17403 } catch (final IllegalArgumentException ignore) {
17404 }
17405 try {
17406 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17407 true, new Matrix(1, 3),
17408 this);
17409 fail("IllegalArgumentException expected but not thrown");
17410 } catch (final IllegalArgumentException ignore) {
17411 }
17412 assertNull(calibrator);
17413 }
17414
17415 @Test
17416 public void testConstructor147() throws WrongSizeException {
17417 final Collection<StandardDeviationBodyKinematics> measurements =
17418 Collections.emptyList();
17419
17420 final Matrix ba = generateBa();
17421 final double[] bias = ba.getBuffer();
17422 final double biasX = ba.getElementAtIndex(0);
17423 final double biasY = ba.getElementAtIndex(1);
17424 final double biasZ = ba.getElementAtIndex(2);
17425
17426 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17427 final double latitude = Math.toRadians(
17428 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17429 final double longitude = Math.toRadians(
17430 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17431 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17432 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17433 final NEDVelocity nedVelocity = new NEDVelocity();
17434 final ECEFPosition ecefPosition = new ECEFPosition();
17435 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17436 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17437 ecefPosition, ecefVelocity);
17438
17439 KnownPositionAccelerometerCalibrator calibrator =
17440 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
17441 true, ba);
17442
17443
17444 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17445 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17446 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17447 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17448 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17449 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17450 final Acceleration bx2 = new Acceleration(0.0,
17451 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17452 calibrator.getInitialBiasXAsAcceleration(bx2);
17453 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17454 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17455 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17456 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17457 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17458 final Acceleration by2 = new Acceleration(0.0,
17459 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17460 calibrator.getInitialBiasYAsAcceleration(by2);
17461 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17462 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17463 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17464 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17465 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17466 final Acceleration bz2 = new Acceleration(0.0,
17467 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17468 calibrator.getInitialBiasZAsAcceleration(bz2);
17469 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17470 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17471 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17472 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17473 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17474 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17475 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17476 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17477 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17478 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17479 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17480 final double[] bias1 = calibrator.getInitialBias();
17481 assertArrayEquals(bias1, bias, 0.0);
17482 final double[] bias2 = new double[3];
17483 calibrator.getInitialBias(bias2);
17484 assertArrayEquals(bias1, bias2, 0.0);
17485 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17486 assertEquals(b1, ba);
17487 final Matrix b2 = new Matrix(3, 1);
17488 calibrator.getInitialBiasAsMatrix(b2);
17489 assertEquals(b1, b2);
17490 final Matrix ma1 = calibrator.getInitialMa();
17491 assertEquals(ma1, new Matrix(3, 3));
17492 final Matrix ma2 = new Matrix(3, 3);
17493 calibrator.getInitialMa(ma2);
17494 assertEquals(ma1, ma2);
17495 assertSame(calibrator.getEcefPosition(), ecefPosition);
17496 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17497 final NEDPosition nedPosition1 = new NEDPosition();
17498 assertTrue(calibrator.getNedPosition(nedPosition1));
17499 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17500 assertSame(calibrator.getMeasurements(), measurements);
17501 assertTrue(calibrator.isCommonAxisUsed());
17502 assertNull(calibrator.getListener());
17503 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17504 assertFalse(calibrator.isReady());
17505 assertFalse(calibrator.isRunning());
17506 assertNull(calibrator.getEstimatedBiases());
17507 assertFalse(calibrator.getEstimatedBiases(null));
17508 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17509 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17510 assertNull(calibrator.getEstimatedBiasFx());
17511 assertNull(calibrator.getEstimatedBiasFy());
17512 assertNull(calibrator.getEstimatedBiasFz());
17513 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17514 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17515 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17516 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17517 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17518 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17519 assertNull(calibrator.getEstimatedMa());
17520 assertNull(calibrator.getEstimatedSx());
17521 assertNull(calibrator.getEstimatedSy());
17522 assertNull(calibrator.getEstimatedSz());
17523 assertNull(calibrator.getEstimatedMxy());
17524 assertNull(calibrator.getEstimatedMxz());
17525 assertNull(calibrator.getEstimatedMyx());
17526 assertNull(calibrator.getEstimatedMyz());
17527 assertNull(calibrator.getEstimatedMzx());
17528 assertNull(calibrator.getEstimatedMzy());
17529 assertNull(calibrator.getEstimatedCovariance());
17530 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17531
17532
17533 calibrator = null;
17534 try {
17535 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17536 measurements, true,
17537 new Matrix(1, 1));
17538 fail("IllegalArgumentException expected but not thrown");
17539 } catch (final IllegalArgumentException ignore) {
17540 }
17541 try {
17542 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17543 measurements,true,
17544 new Matrix(1, 3));
17545 fail("IllegalArgumentException expected but not thrown");
17546 } catch (final IllegalArgumentException ignore) {
17547 }
17548 assertNull(calibrator);
17549 }
17550
17551 @Test
17552 public void testConstructor148() throws WrongSizeException {
17553 final Collection<StandardDeviationBodyKinematics> measurements =
17554 Collections.emptyList();
17555
17556 final Matrix ba = generateBa();
17557 final double[] bias = ba.getBuffer();
17558 final double biasX = ba.getElementAtIndex(0);
17559 final double biasY = ba.getElementAtIndex(1);
17560 final double biasZ = ba.getElementAtIndex(2);
17561
17562 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17563 final double latitude = Math.toRadians(
17564 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17565 final double longitude = Math.toRadians(
17566 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17567 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17568 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17569 final NEDVelocity nedVelocity = new NEDVelocity();
17570 final ECEFPosition ecefPosition = new ECEFPosition();
17571 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17572 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17573 ecefPosition, ecefVelocity);
17574
17575 KnownPositionAccelerometerCalibrator calibrator =
17576 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
17577 true, ba, this);
17578
17579
17580 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17581 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17582 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17583 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17584 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17585 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17586 final Acceleration bx2 = new Acceleration(0.0,
17587 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17588 calibrator.getInitialBiasXAsAcceleration(bx2);
17589 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17590 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17591 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17592 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17593 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17594 final Acceleration by2 = new Acceleration(0.0,
17595 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17596 calibrator.getInitialBiasYAsAcceleration(by2);
17597 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17598 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17599 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17600 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17601 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17602 final Acceleration bz2 = new Acceleration(0.0,
17603 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17604 calibrator.getInitialBiasZAsAcceleration(bz2);
17605 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17606 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17607 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17608 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17609 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17610 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17611 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17612 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17613 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17614 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17615 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17616 final double[] bias1 = calibrator.getInitialBias();
17617 assertArrayEquals(bias1, bias, 0.0);
17618 final double[] bias2 = new double[3];
17619 calibrator.getInitialBias(bias2);
17620 assertArrayEquals(bias1, bias2, 0.0);
17621 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17622 assertEquals(b1, ba);
17623 final Matrix b2 = new Matrix(3, 1);
17624 calibrator.getInitialBiasAsMatrix(b2);
17625 assertEquals(b1, b2);
17626 final Matrix ma1 = calibrator.getInitialMa();
17627 assertEquals(ma1, new Matrix(3, 3));
17628 final Matrix ma2 = new Matrix(3, 3);
17629 calibrator.getInitialMa(ma2);
17630 assertEquals(ma1, ma2);
17631 assertSame(calibrator.getEcefPosition(), ecefPosition);
17632 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17633 final NEDPosition nedPosition1 = new NEDPosition();
17634 assertTrue(calibrator.getNedPosition(nedPosition1));
17635 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17636 assertSame(calibrator.getMeasurements(), measurements);
17637 assertTrue(calibrator.isCommonAxisUsed());
17638 assertSame(calibrator.getListener(), this);
17639 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17640 assertFalse(calibrator.isReady());
17641 assertFalse(calibrator.isRunning());
17642 assertNull(calibrator.getEstimatedBiases());
17643 assertFalse(calibrator.getEstimatedBiases(null));
17644 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17645 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17646 assertNull(calibrator.getEstimatedBiasFx());
17647 assertNull(calibrator.getEstimatedBiasFy());
17648 assertNull(calibrator.getEstimatedBiasFz());
17649 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17650 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17651 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17652 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17653 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17654 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17655 assertNull(calibrator.getEstimatedMa());
17656 assertNull(calibrator.getEstimatedSx());
17657 assertNull(calibrator.getEstimatedSy());
17658 assertNull(calibrator.getEstimatedSz());
17659 assertNull(calibrator.getEstimatedMxy());
17660 assertNull(calibrator.getEstimatedMxz());
17661 assertNull(calibrator.getEstimatedMyx());
17662 assertNull(calibrator.getEstimatedMyz());
17663 assertNull(calibrator.getEstimatedMzx());
17664 assertNull(calibrator.getEstimatedMzy());
17665 assertNull(calibrator.getEstimatedCovariance());
17666 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17667
17668
17669 calibrator = null;
17670 try {
17671 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17672 measurements, true,
17673 new Matrix(1, 1), this);
17674 fail("IllegalArgumentException expected but not thrown");
17675 } catch (final IllegalArgumentException ignore) {
17676 }
17677 try {
17678 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17679 measurements,true,
17680 new Matrix(1, 3), this);
17681 fail("IllegalArgumentException expected but not thrown");
17682 } catch (final IllegalArgumentException ignore) {
17683 }
17684 assertNull(calibrator);
17685 }
17686
17687 @Test
17688 public void testConstructor149() throws WrongSizeException {
17689 final Matrix ba = generateBa();
17690 final double[] bias = ba.getBuffer();
17691 final double biasX = ba.getElementAtIndex(0);
17692 final double biasY = ba.getElementAtIndex(1);
17693 final double biasZ = ba.getElementAtIndex(2);
17694
17695 final Matrix ma = generateMaCommonAxis();
17696 final double sx = ma.getElementAt(0, 0);
17697 final double sy = ma.getElementAt(1, 1);
17698 final double sz = ma.getElementAt(2, 2);
17699 final double mxy = ma.getElementAt(0, 1);
17700 final double mxz = ma.getElementAt(0, 2);
17701 final double myx = ma.getElementAt(1, 0);
17702 final double myz = ma.getElementAt(1, 2);
17703 final double mzx = ma.getElementAt(2, 0);
17704 final double mzy = ma.getElementAt(2, 1);
17705
17706 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17707 final double latitude = Math.toRadians(
17708 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17709 final double longitude = Math.toRadians(
17710 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17711 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17712 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17713 final NEDVelocity nedVelocity = new NEDVelocity();
17714 final ECEFPosition ecefPosition = new ECEFPosition();
17715 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17716 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17717 ecefPosition, ecefVelocity);
17718
17719 KnownPositionAccelerometerCalibrator calibrator =
17720 new KnownPositionAccelerometerCalibrator(ecefPosition, ba, ma);
17721
17722
17723 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17724 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17725 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17726 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17727 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17728 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17729 final Acceleration bx2 = new Acceleration(0.0,
17730 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17731 calibrator.getInitialBiasXAsAcceleration(bx2);
17732 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17733 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17734 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17735 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17736 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17737 final Acceleration by2 = new Acceleration(0.0,
17738 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17739 calibrator.getInitialBiasYAsAcceleration(by2);
17740 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17741 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17742 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17743 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17744 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17745 final Acceleration bz2 = new Acceleration(0.0,
17746 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17747 calibrator.getInitialBiasZAsAcceleration(bz2);
17748 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17749 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17750 assertEquals(calibrator.getInitialSx(), sx, 0.0);
17751 assertEquals(calibrator.getInitialSy(), sy, 0.0);
17752 assertEquals(calibrator.getInitialSz(), sz, 0.0);
17753 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
17754 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
17755 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
17756 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
17757 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
17758 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
17759 final double[] bias1 = calibrator.getInitialBias();
17760 assertArrayEquals(bias1, bias, 0.0);
17761 final double[] bias2 = new double[3];
17762 calibrator.getInitialBias(bias2);
17763 assertArrayEquals(bias1, bias2, 0.0);
17764 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17765 assertEquals(b1, ba);
17766 final Matrix b2 = new Matrix(3, 1);
17767 calibrator.getInitialBiasAsMatrix(b2);
17768 assertEquals(b1, b2);
17769 final Matrix ma1 = new Matrix(3, 3);
17770 ma1.setSubmatrix(0, 0,
17771 2, 2,
17772 new double[]{ sx, myx, mzx,
17773 mxy, sy, mzy,
17774 mxz, myz, sz});
17775 assertEquals(calibrator.getInitialMa(), ma1);
17776 final Matrix ma2 = new Matrix(3, 3);
17777 calibrator.getInitialMa(ma2);
17778 assertEquals(ma1, ma2);
17779 assertSame(calibrator.getEcefPosition(), ecefPosition);
17780 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17781 final NEDPosition nedPosition1 = new NEDPosition();
17782 assertTrue(calibrator.getNedPosition(nedPosition1));
17783 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17784 assertNull(calibrator.getMeasurements());
17785 assertFalse(calibrator.isCommonAxisUsed());
17786 assertNull(calibrator.getListener());
17787 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
17788 assertFalse(calibrator.isReady());
17789 assertFalse(calibrator.isRunning());
17790 assertNull(calibrator.getEstimatedBiases());
17791 assertFalse(calibrator.getEstimatedBiases(null));
17792 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17793 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17794 assertNull(calibrator.getEstimatedBiasFx());
17795 assertNull(calibrator.getEstimatedBiasFy());
17796 assertNull(calibrator.getEstimatedBiasFz());
17797 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17798 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17799 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17800 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17801 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17802 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17803 assertNull(calibrator.getEstimatedMa());
17804 assertNull(calibrator.getEstimatedSx());
17805 assertNull(calibrator.getEstimatedSy());
17806 assertNull(calibrator.getEstimatedSz());
17807 assertNull(calibrator.getEstimatedMxy());
17808 assertNull(calibrator.getEstimatedMxz());
17809 assertNull(calibrator.getEstimatedMyx());
17810 assertNull(calibrator.getEstimatedMyz());
17811 assertNull(calibrator.getEstimatedMzx());
17812 assertNull(calibrator.getEstimatedMzy());
17813 assertNull(calibrator.getEstimatedCovariance());
17814 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17815
17816
17817 calibrator = null;
17818 try {
17819 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17820 new Matrix(1, 1), ma);
17821 fail("IllegalArgumentException expected but not thrown");
17822 } catch (final IllegalArgumentException ignore) {
17823 }
17824 try {
17825 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17826 new Matrix(1, 3), ma);
17827 fail("IllegalArgumentException expected but not thrown");
17828 } catch (final IllegalArgumentException ignore) {
17829 }
17830 try {
17831 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17832 ba, new Matrix(1, 3));
17833 fail("IllegalArgumentException expected but not thrown");
17834 } catch (final IllegalArgumentException ignore) {
17835 }
17836 try {
17837 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17838 ba, new Matrix(3, 1));
17839 fail("IllegalArgumentException expected but not thrown");
17840 } catch (final IllegalArgumentException ignore) {
17841 }
17842 assertNull(calibrator);
17843 }
17844
17845 @Test
17846 public void testConstructor150() throws WrongSizeException {
17847 final Matrix ba = generateBa();
17848 final double[] bias = ba.getBuffer();
17849 final double biasX = ba.getElementAtIndex(0);
17850 final double biasY = ba.getElementAtIndex(1);
17851 final double biasZ = ba.getElementAtIndex(2);
17852
17853 final Matrix ma = generateMaCommonAxis();
17854 final double sx = ma.getElementAt(0, 0);
17855 final double sy = ma.getElementAt(1, 1);
17856 final double sz = ma.getElementAt(2, 2);
17857 final double mxy = ma.getElementAt(0, 1);
17858 final double mxz = ma.getElementAt(0, 2);
17859 final double myx = ma.getElementAt(1, 0);
17860 final double myz = ma.getElementAt(1, 2);
17861 final double mzx = ma.getElementAt(2, 0);
17862 final double mzy = ma.getElementAt(2, 1);
17863
17864 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17865 final double latitude = Math.toRadians(
17866 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17867 final double longitude = Math.toRadians(
17868 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17869 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17870 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17871 final NEDVelocity nedVelocity = new NEDVelocity();
17872 final ECEFPosition ecefPosition = new ECEFPosition();
17873 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17874 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17875 ecefPosition, ecefVelocity);
17876
17877 KnownPositionAccelerometerCalibrator calibrator =
17878 new KnownPositionAccelerometerCalibrator(ecefPosition,
17879 ba, ma, this);
17880
17881
17882 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17883 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17884 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17885 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17886 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17887 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17888 final Acceleration bx2 = new Acceleration(0.0,
17889 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17890 calibrator.getInitialBiasXAsAcceleration(bx2);
17891 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17892 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17893 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17894 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17895 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17896 final Acceleration by2 = new Acceleration(0.0,
17897 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17898 calibrator.getInitialBiasYAsAcceleration(by2);
17899 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17900 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17901 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17902 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17903 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17904 final Acceleration bz2 = new Acceleration(0.0,
17905 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17906 calibrator.getInitialBiasZAsAcceleration(bz2);
17907 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17908 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17909 assertEquals(calibrator.getInitialSx(), sx, 0.0);
17910 assertEquals(calibrator.getInitialSy(), sy, 0.0);
17911 assertEquals(calibrator.getInitialSz(), sz, 0.0);
17912 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
17913 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
17914 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
17915 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
17916 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
17917 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
17918 final double[] bias1 = calibrator.getInitialBias();
17919 assertArrayEquals(bias1, bias, 0.0);
17920 final double[] bias2 = new double[3];
17921 calibrator.getInitialBias(bias2);
17922 assertArrayEquals(bias1, bias2, 0.0);
17923 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17924 assertEquals(b1, ba);
17925 final Matrix b2 = new Matrix(3, 1);
17926 calibrator.getInitialBiasAsMatrix(b2);
17927 assertEquals(b1, b2);
17928 final Matrix ma1 = new Matrix(3, 3);
17929 ma1.setSubmatrix(0, 0,
17930 2, 2,
17931 new double[]{ sx, myx, mzx,
17932 mxy, sy, mzy,
17933 mxz, myz, sz});
17934 assertEquals(calibrator.getInitialMa(), ma1);
17935 final Matrix ma2 = new Matrix(3, 3);
17936 calibrator.getInitialMa(ma2);
17937 assertEquals(ma1, ma2);
17938 assertSame(calibrator.getEcefPosition(), ecefPosition);
17939 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17940 final NEDPosition nedPosition1 = new NEDPosition();
17941 assertTrue(calibrator.getNedPosition(nedPosition1));
17942 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17943 assertNull(calibrator.getMeasurements());
17944 assertFalse(calibrator.isCommonAxisUsed());
17945 assertSame(calibrator.getListener(), this);
17946 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
17947 assertFalse(calibrator.isReady());
17948 assertFalse(calibrator.isRunning());
17949 assertNull(calibrator.getEstimatedBiases());
17950 assertFalse(calibrator.getEstimatedBiases(null));
17951 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17952 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17953 assertNull(calibrator.getEstimatedBiasFx());
17954 assertNull(calibrator.getEstimatedBiasFy());
17955 assertNull(calibrator.getEstimatedBiasFz());
17956 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17957 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17958 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17959 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17960 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17961 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17962 assertNull(calibrator.getEstimatedMa());
17963 assertNull(calibrator.getEstimatedSx());
17964 assertNull(calibrator.getEstimatedSy());
17965 assertNull(calibrator.getEstimatedSz());
17966 assertNull(calibrator.getEstimatedMxy());
17967 assertNull(calibrator.getEstimatedMxz());
17968 assertNull(calibrator.getEstimatedMyx());
17969 assertNull(calibrator.getEstimatedMyz());
17970 assertNull(calibrator.getEstimatedMzx());
17971 assertNull(calibrator.getEstimatedMzy());
17972 assertNull(calibrator.getEstimatedCovariance());
17973 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17974
17975
17976 calibrator = null;
17977 try {
17978 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17979 new Matrix(1, 1), ma, this);
17980 fail("IllegalArgumentException expected but not thrown");
17981 } catch (final IllegalArgumentException ignore) {
17982 }
17983 try {
17984 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17985 new Matrix(1, 3), ma, this);
17986 fail("IllegalArgumentException expected but not thrown");
17987 } catch (final IllegalArgumentException ignore) {
17988 }
17989 try {
17990 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17991 ba, new Matrix(1, 3), this);
17992 fail("IllegalArgumentException expected but not thrown");
17993 } catch (final IllegalArgumentException ignore) {
17994 }
17995 try {
17996 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17997 ba, new Matrix(3, 1), this);
17998 fail("IllegalArgumentException expected but not thrown");
17999 } catch (final IllegalArgumentException ignore) {
18000 }
18001 assertNull(calibrator);
18002 }
18003
18004 @Test
18005 public void testConstructor151() throws WrongSizeException {
18006 final Collection<StandardDeviationBodyKinematics> measurements =
18007 Collections.emptyList();
18008
18009 final Matrix ba = generateBa();
18010 final double[] bias = ba.getBuffer();
18011 final double biasX = ba.getElementAtIndex(0);
18012 final double biasY = ba.getElementAtIndex(1);
18013 final double biasZ = ba.getElementAtIndex(2);
18014
18015 final Matrix ma = generateMaCommonAxis();
18016 final double sx = ma.getElementAt(0, 0);
18017 final double sy = ma.getElementAt(1, 1);
18018 final double sz = ma.getElementAt(2, 2);
18019 final double mxy = ma.getElementAt(0, 1);
18020 final double mxz = ma.getElementAt(0, 2);
18021 final double myx = ma.getElementAt(1, 0);
18022 final double myz = ma.getElementAt(1, 2);
18023 final double mzx = ma.getElementAt(2, 0);
18024 final double mzy = ma.getElementAt(2, 1);
18025
18026 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18027 final double latitude = Math.toRadians(
18028 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18029 final double longitude = Math.toRadians(
18030 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18031 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18032 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18033 final NEDVelocity nedVelocity = new NEDVelocity();
18034 final ECEFPosition ecefPosition = new ECEFPosition();
18035 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18036 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18037 ecefPosition, ecefVelocity);
18038
18039 KnownPositionAccelerometerCalibrator calibrator =
18040 new KnownPositionAccelerometerCalibrator(ecefPosition,
18041 measurements, ba, ma);
18042
18043
18044 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18045 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18046 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18047 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18048 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18049 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18050 final Acceleration bx2 = new Acceleration(0.0,
18051 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18052 calibrator.getInitialBiasXAsAcceleration(bx2);
18053 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18054 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18055 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18056 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18057 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18058 final Acceleration by2 = new Acceleration(0.0,
18059 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18060 calibrator.getInitialBiasYAsAcceleration(by2);
18061 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18062 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18063 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18064 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18065 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18066 final Acceleration bz2 = new Acceleration(0.0,
18067 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18068 calibrator.getInitialBiasZAsAcceleration(bz2);
18069 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18070 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18071 assertEquals(calibrator.getInitialSx(), sx, 0.0);
18072 assertEquals(calibrator.getInitialSy(), sy, 0.0);
18073 assertEquals(calibrator.getInitialSz(), sz, 0.0);
18074 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
18075 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
18076 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
18077 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
18078 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
18079 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
18080 final double[] bias1 = calibrator.getInitialBias();
18081 assertArrayEquals(bias1, bias, 0.0);
18082 final double[] bias2 = new double[3];
18083 calibrator.getInitialBias(bias2);
18084 assertArrayEquals(bias1, bias2, 0.0);
18085 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18086 assertEquals(b1, ba);
18087 final Matrix b2 = new Matrix(3, 1);
18088 calibrator.getInitialBiasAsMatrix(b2);
18089 assertEquals(b1, b2);
18090 final Matrix ma1 = new Matrix(3, 3);
18091 ma1.setSubmatrix(0, 0,
18092 2, 2,
18093 new double[]{ sx, myx, mzx,
18094 mxy, sy, mzy,
18095 mxz, myz, sz});
18096 assertEquals(calibrator.getInitialMa(), ma1);
18097 final Matrix ma2 = new Matrix(3, 3);
18098 calibrator.getInitialMa(ma2);
18099 assertEquals(ma1, ma2);
18100 assertSame(calibrator.getEcefPosition(), ecefPosition);
18101 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18102 final NEDPosition nedPosition1 = new NEDPosition();
18103 assertTrue(calibrator.getNedPosition(nedPosition1));
18104 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18105 assertSame(calibrator.getMeasurements(), measurements);
18106 assertFalse(calibrator.isCommonAxisUsed());
18107 assertNull(calibrator.getListener());
18108 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
18109 assertFalse(calibrator.isReady());
18110 assertFalse(calibrator.isRunning());
18111 assertNull(calibrator.getEstimatedBiases());
18112 assertFalse(calibrator.getEstimatedBiases(null));
18113 assertNull(calibrator.getEstimatedBiasesAsMatrix());
18114 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18115 assertNull(calibrator.getEstimatedBiasFx());
18116 assertNull(calibrator.getEstimatedBiasFy());
18117 assertNull(calibrator.getEstimatedBiasFz());
18118 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18119 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18120 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18121 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18122 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18123 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18124 assertNull(calibrator.getEstimatedMa());
18125 assertNull(calibrator.getEstimatedSx());
18126 assertNull(calibrator.getEstimatedSy());
18127 assertNull(calibrator.getEstimatedSz());
18128 assertNull(calibrator.getEstimatedMxy());
18129 assertNull(calibrator.getEstimatedMxz());
18130 assertNull(calibrator.getEstimatedMyx());
18131 assertNull(calibrator.getEstimatedMyz());
18132 assertNull(calibrator.getEstimatedMzx());
18133 assertNull(calibrator.getEstimatedMzy());
18134 assertNull(calibrator.getEstimatedCovariance());
18135 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18136
18137
18138 calibrator = null;
18139 try {
18140 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18141 measurements, new Matrix(1, 1), ma);
18142 fail("IllegalArgumentException expected but not thrown");
18143 } catch (final IllegalArgumentException ignore) {
18144 }
18145 try {
18146 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18147 measurements, new Matrix(1, 3), ma);
18148 fail("IllegalArgumentException expected but not thrown");
18149 } catch (final IllegalArgumentException ignore) {
18150 }
18151 try {
18152 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18153 measurements, ba, new Matrix(1, 3));
18154 fail("IllegalArgumentException expected but not thrown");
18155 } catch (final IllegalArgumentException ignore) {
18156 }
18157 try {
18158 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18159 measurements, ba, new Matrix(3, 1));
18160 fail("IllegalArgumentException expected but not thrown");
18161 } catch (final IllegalArgumentException ignore) {
18162 }
18163 assertNull(calibrator);
18164 }
18165
18166 @Test
18167 public void testConstructor152() throws WrongSizeException {
18168 final Collection<StandardDeviationBodyKinematics> measurements =
18169 Collections.emptyList();
18170
18171 final Matrix ba = generateBa();
18172 final double[] bias = ba.getBuffer();
18173 final double biasX = ba.getElementAtIndex(0);
18174 final double biasY = ba.getElementAtIndex(1);
18175 final double biasZ = ba.getElementAtIndex(2);
18176
18177 final Matrix ma = generateMaCommonAxis();
18178 final double sx = ma.getElementAt(0, 0);
18179 final double sy = ma.getElementAt(1, 1);
18180 final double sz = ma.getElementAt(2, 2);
18181 final double mxy = ma.getElementAt(0, 1);
18182 final double mxz = ma.getElementAt(0, 2);
18183 final double myx = ma.getElementAt(1, 0);
18184 final double myz = ma.getElementAt(1, 2);
18185 final double mzx = ma.getElementAt(2, 0);
18186 final double mzy = ma.getElementAt(2, 1);
18187
18188 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18189 final double latitude = Math.toRadians(
18190 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18191 final double longitude = Math.toRadians(
18192 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18193 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18194 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18195 final NEDVelocity nedVelocity = new NEDVelocity();
18196 final ECEFPosition ecefPosition = new ECEFPosition();
18197 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18198 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18199 ecefPosition, ecefVelocity);
18200
18201 KnownPositionAccelerometerCalibrator calibrator =
18202 new KnownPositionAccelerometerCalibrator(ecefPosition,
18203 measurements, ba, ma, this);
18204
18205
18206 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18207 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18208 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18209 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18210 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18211 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18212 final Acceleration bx2 = new Acceleration(0.0,
18213 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18214 calibrator.getInitialBiasXAsAcceleration(bx2);
18215 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18216 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18217 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18218 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18219 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18220 final Acceleration by2 = new Acceleration(0.0,
18221 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18222 calibrator.getInitialBiasYAsAcceleration(by2);
18223 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18224 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18225 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18226 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18227 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18228 final Acceleration bz2 = new Acceleration(0.0,
18229 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18230 calibrator.getInitialBiasZAsAcceleration(bz2);
18231 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18232 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18233 assertEquals(calibrator.getInitialSx(), sx, 0.0);
18234 assertEquals(calibrator.getInitialSy(), sy, 0.0);
18235 assertEquals(calibrator.getInitialSz(), sz, 0.0);
18236 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
18237 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
18238 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
18239 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
18240 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
18241 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
18242 final double[] bias1 = calibrator.getInitialBias();
18243 assertArrayEquals(bias1, bias, 0.0);
18244 final double[] bias2 = new double[3];
18245 calibrator.getInitialBias(bias2);
18246 assertArrayEquals(bias1, bias2, 0.0);
18247 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18248 assertEquals(b1, ba);
18249 final Matrix b2 = new Matrix(3, 1);
18250 calibrator.getInitialBiasAsMatrix(b2);
18251 assertEquals(b1, b2);
18252 final Matrix ma1 = new Matrix(3, 3);
18253 ma1.setSubmatrix(0, 0,
18254 2, 2,
18255 new double[]{ sx, myx, mzx,
18256 mxy, sy, mzy,
18257 mxz, myz, sz});
18258 assertEquals(calibrator.getInitialMa(), ma1);
18259 final Matrix ma2 = new Matrix(3, 3);
18260 calibrator.getInitialMa(ma2);
18261 assertEquals(ma1, ma2);
18262 assertSame(calibrator.getEcefPosition(), ecefPosition);
18263 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18264 final NEDPosition nedPosition1 = new NEDPosition();
18265 assertTrue(calibrator.getNedPosition(nedPosition1));
18266 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18267 assertSame(calibrator.getMeasurements(), measurements);
18268 assertFalse(calibrator.isCommonAxisUsed());
18269 assertSame(calibrator.getListener(), this);
18270 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
18271 assertFalse(calibrator.isReady());
18272 assertFalse(calibrator.isRunning());
18273 assertNull(calibrator.getEstimatedBiases());
18274 assertFalse(calibrator.getEstimatedBiases(null));
18275 assertNull(calibrator.getEstimatedBiasesAsMatrix());
18276 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18277 assertNull(calibrator.getEstimatedBiasFx());
18278 assertNull(calibrator.getEstimatedBiasFy());
18279 assertNull(calibrator.getEstimatedBiasFz());
18280 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18281 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18282 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18283 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18284 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18285 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18286 assertNull(calibrator.getEstimatedMa());
18287 assertNull(calibrator.getEstimatedSx());
18288 assertNull(calibrator.getEstimatedSy());
18289 assertNull(calibrator.getEstimatedSz());
18290 assertNull(calibrator.getEstimatedMxy());
18291 assertNull(calibrator.getEstimatedMxz());
18292 assertNull(calibrator.getEstimatedMyx());
18293 assertNull(calibrator.getEstimatedMyz());
18294 assertNull(calibrator.getEstimatedMzx());
18295 assertNull(calibrator.getEstimatedMzy());
18296 assertNull(calibrator.getEstimatedCovariance());
18297 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18298
18299
18300 calibrator = null;
18301 try {
18302 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18303 measurements, new Matrix(1, 1), ma, this);
18304 fail("IllegalArgumentException expected but not thrown");
18305 } catch (final IllegalArgumentException ignore) {
18306 }
18307 try {
18308 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18309 measurements, new Matrix(1, 3), ma, this);
18310 fail("IllegalArgumentException expected but not thrown");
18311 } catch (final IllegalArgumentException ignore) {
18312 }
18313 try {
18314 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18315 measurements, ba, new Matrix(1, 3), this);
18316 fail("IllegalArgumentException expected but not thrown");
18317 } catch (final IllegalArgumentException ignore) {
18318 }
18319 try {
18320 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18321 measurements, ba, new Matrix(3, 1), this);
18322 fail("IllegalArgumentException expected but not thrown");
18323 } catch (final IllegalArgumentException ignore) {
18324 }
18325 assertNull(calibrator);
18326 }
18327
18328 @Test
18329 public void testConstructor153() throws WrongSizeException {
18330 final Matrix ba = generateBa();
18331 final double[] bias = ba.getBuffer();
18332 final double biasX = ba.getElementAtIndex(0);
18333 final double biasY = ba.getElementAtIndex(1);
18334 final double biasZ = ba.getElementAtIndex(2);
18335
18336 final Matrix ma = generateMaCommonAxis();
18337 final double sx = ma.getElementAt(0, 0);
18338 final double sy = ma.getElementAt(1, 1);
18339 final double sz = ma.getElementAt(2, 2);
18340 final double mxy = ma.getElementAt(0, 1);
18341 final double mxz = ma.getElementAt(0, 2);
18342 final double myx = ma.getElementAt(1, 0);
18343 final double myz = ma.getElementAt(1, 2);
18344 final double mzx = ma.getElementAt(2, 0);
18345 final double mzy = ma.getElementAt(2, 1);
18346
18347 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18348 final double latitude = Math.toRadians(
18349 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18350 final double longitude = Math.toRadians(
18351 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18352 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18353 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18354 final NEDVelocity nedVelocity = new NEDVelocity();
18355 final ECEFPosition ecefPosition = new ECEFPosition();
18356 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18357 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18358 ecefPosition, ecefVelocity);
18359
18360 KnownPositionAccelerometerCalibrator calibrator =
18361 new KnownPositionAccelerometerCalibrator(ecefPosition,
18362 true, ba, ma);
18363
18364
18365 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18366 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18367 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18368 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18369 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18370 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18371 final Acceleration bx2 = new Acceleration(0.0,
18372 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18373 calibrator.getInitialBiasXAsAcceleration(bx2);
18374 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18375 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18376 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18377 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18378 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18379 final Acceleration by2 = new Acceleration(0.0,
18380 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18381 calibrator.getInitialBiasYAsAcceleration(by2);
18382 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18383 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18384 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18385 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18386 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18387 final Acceleration bz2 = new Acceleration(0.0,
18388 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18389 calibrator.getInitialBiasZAsAcceleration(bz2);
18390 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18391 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18392 assertEquals(calibrator.getInitialSx(), sx, 0.0);
18393 assertEquals(calibrator.getInitialSy(), sy, 0.0);
18394 assertEquals(calibrator.getInitialSz(), sz, 0.0);
18395 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
18396 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
18397 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
18398 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
18399 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
18400 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
18401 final double[] bias1 = calibrator.getInitialBias();
18402 assertArrayEquals(bias1, bias, 0.0);
18403 final double[] bias2 = new double[3];
18404 calibrator.getInitialBias(bias2);
18405 assertArrayEquals(bias1, bias2, 0.0);
18406 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18407 assertEquals(b1, ba);
18408 final Matrix b2 = new Matrix(3, 1);
18409 calibrator.getInitialBiasAsMatrix(b2);
18410 assertEquals(b1, b2);
18411 final Matrix ma1 = new Matrix(3, 3);
18412 ma1.setSubmatrix(0, 0,
18413 2, 2,
18414 new double[]{ sx, myx, mzx,
18415 mxy, sy, mzy,
18416 mxz, myz, sz});
18417 assertEquals(calibrator.getInitialMa(), ma1);
18418 final Matrix ma2 = new Matrix(3, 3);
18419 calibrator.getInitialMa(ma2);
18420 assertEquals(ma1, ma2);
18421 assertSame(calibrator.getEcefPosition(), ecefPosition);
18422 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18423 final NEDPosition nedPosition1 = new NEDPosition();
18424 assertTrue(calibrator.getNedPosition(nedPosition1));
18425 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18426 assertNull(calibrator.getMeasurements());
18427 assertTrue(calibrator.isCommonAxisUsed());
18428 assertNull(calibrator.getListener());
18429 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18430 assertFalse(calibrator.isReady());
18431 assertFalse(calibrator.isRunning());
18432 assertNull(calibrator.getEstimatedBiases());
18433 assertFalse(calibrator.getEstimatedBiases(null));
18434 assertNull(calibrator.getEstimatedBiasesAsMatrix());
18435 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18436 assertNull(calibrator.getEstimatedBiasFx());
18437 assertNull(calibrator.getEstimatedBiasFy());
18438 assertNull(calibrator.getEstimatedBiasFz());
18439 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18440 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18441 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18442 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18443 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18444 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18445 assertNull(calibrator.getEstimatedMa());
18446 assertNull(calibrator.getEstimatedSx());
18447 assertNull(calibrator.getEstimatedSy());
18448 assertNull(calibrator.getEstimatedSz());
18449 assertNull(calibrator.getEstimatedMxy());
18450 assertNull(calibrator.getEstimatedMxz());
18451 assertNull(calibrator.getEstimatedMyx());
18452 assertNull(calibrator.getEstimatedMyz());
18453 assertNull(calibrator.getEstimatedMzx());
18454 assertNull(calibrator.getEstimatedMzy());
18455 assertNull(calibrator.getEstimatedCovariance());
18456 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18457
18458
18459 calibrator = null;
18460 try {
18461 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18462 true, new Matrix(1, 1), ma);
18463 fail("IllegalArgumentException expected but not thrown");
18464 } catch (final IllegalArgumentException ignore) {
18465 }
18466 try {
18467 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18468 true, new Matrix(1, 3), ma);
18469 fail("IllegalArgumentException expected but not thrown");
18470 } catch (final IllegalArgumentException ignore) {
18471 }
18472 try {
18473 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18474 true, ba, new Matrix(1, 3));
18475 fail("IllegalArgumentException expected but not thrown");
18476 } catch (final IllegalArgumentException ignore) {
18477 }
18478 try {
18479 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18480 true, ba, new Matrix(3, 1));
18481 fail("IllegalArgumentException expected but not thrown");
18482 } catch (final IllegalArgumentException ignore) {
18483 }
18484 assertNull(calibrator);
18485 }
18486
18487 @Test
18488 public void testConstructor154() throws WrongSizeException {
18489 final Matrix ba = generateBa();
18490 final double[] bias = ba.getBuffer();
18491 final double biasX = ba.getElementAtIndex(0);
18492 final double biasY = ba.getElementAtIndex(1);
18493 final double biasZ = ba.getElementAtIndex(2);
18494
18495 final Matrix ma = generateMaCommonAxis();
18496 final double sx = ma.getElementAt(0, 0);
18497 final double sy = ma.getElementAt(1, 1);
18498 final double sz = ma.getElementAt(2, 2);
18499 final double mxy = ma.getElementAt(0, 1);
18500 final double mxz = ma.getElementAt(0, 2);
18501 final double myx = ma.getElementAt(1, 0);
18502 final double myz = ma.getElementAt(1, 2);
18503 final double mzx = ma.getElementAt(2, 0);
18504 final double mzy = ma.getElementAt(2, 1);
18505
18506 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18507 final double latitude = Math.toRadians(
18508 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18509 final double longitude = Math.toRadians(
18510 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18511 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18512 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18513 final NEDVelocity nedVelocity = new NEDVelocity();
18514 final ECEFPosition ecefPosition = new ECEFPosition();
18515 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18516 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18517 ecefPosition, ecefVelocity);
18518
18519 KnownPositionAccelerometerCalibrator calibrator =
18520 new KnownPositionAccelerometerCalibrator(ecefPosition,
18521 true, ba, ma, this);
18522
18523
18524 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18525 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18526 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18527 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18528 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18529 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18530 final Acceleration bx2 = new Acceleration(0.0,
18531 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18532 calibrator.getInitialBiasXAsAcceleration(bx2);
18533 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18534 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18535 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18536 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18537 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18538 final Acceleration by2 = new Acceleration(0.0,
18539 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18540 calibrator.getInitialBiasYAsAcceleration(by2);
18541 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18542 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18543 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18544 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18545 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18546 final Acceleration bz2 = new Acceleration(0.0,
18547 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18548 calibrator.getInitialBiasZAsAcceleration(bz2);
18549 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18550 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18551 assertEquals(calibrator.getInitialSx(), sx, 0.0);
18552 assertEquals(calibrator.getInitialSy(), sy, 0.0);
18553 assertEquals(calibrator.getInitialSz(), sz, 0.0);
18554 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
18555 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
18556 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
18557 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
18558 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
18559 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
18560 final double[] bias1 = calibrator.getInitialBias();
18561 assertArrayEquals(bias1, bias, 0.0);
18562 final double[] bias2 = new double[3];
18563 calibrator.getInitialBias(bias2);
18564 assertArrayEquals(bias1, bias2, 0.0);
18565 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18566 assertEquals(b1, ba);
18567 final Matrix b2 = new Matrix(3, 1);
18568 calibrator.getInitialBiasAsMatrix(b2);
18569 assertEquals(b1, b2);
18570 final Matrix ma1 = new Matrix(3, 3);
18571 ma1.setSubmatrix(0, 0,
18572 2, 2,
18573 new double[]{ sx, myx, mzx,
18574 mxy, sy, mzy,
18575 mxz, myz, sz});
18576 assertEquals(calibrator.getInitialMa(), ma1);
18577 final Matrix ma2 = new Matrix(3, 3);
18578 calibrator.getInitialMa(ma2);
18579 assertEquals(ma1, ma2);
18580 assertSame(calibrator.getEcefPosition(), ecefPosition);
18581 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18582 final NEDPosition nedPosition1 = new NEDPosition();
18583 assertTrue(calibrator.getNedPosition(nedPosition1));
18584 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18585 assertNull(calibrator.getMeasurements());
18586 assertTrue(calibrator.isCommonAxisUsed());
18587 assertSame(calibrator.getListener(), this);
18588 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18589 assertFalse(calibrator.isReady());
18590 assertFalse(calibrator.isRunning());
18591 assertNull(calibrator.getEstimatedBiases());
18592 assertFalse(calibrator.getEstimatedBiases(null));
18593 assertNull(calibrator.getEstimatedBiasesAsMatrix());
18594 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18595 assertNull(calibrator.getEstimatedBiasFx());
18596 assertNull(calibrator.getEstimatedBiasFy());
18597 assertNull(calibrator.getEstimatedBiasFz());
18598 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18599 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18600 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18601 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18602 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18603 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18604 assertNull(calibrator.getEstimatedMa());
18605 assertNull(calibrator.getEstimatedSx());
18606 assertNull(calibrator.getEstimatedSy());
18607 assertNull(calibrator.getEstimatedSz());
18608 assertNull(calibrator.getEstimatedMxy());
18609 assertNull(calibrator.getEstimatedMxz());
18610 assertNull(calibrator.getEstimatedMyx());
18611 assertNull(calibrator.getEstimatedMyz());
18612 assertNull(calibrator.getEstimatedMzx());
18613 assertNull(calibrator.getEstimatedMzy());
18614 assertNull(calibrator.getEstimatedCovariance());
18615 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18616
18617
18618 calibrator = null;
18619 try {
18620 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18621 true, new Matrix(1, 1), ma, this);
18622 fail("IllegalArgumentException expected but not thrown");
18623 } catch (final IllegalArgumentException ignore) {
18624 }
18625 try {
18626 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18627 true, new Matrix(1, 3), ma, this);
18628 fail("IllegalArgumentException expected but not thrown");
18629 } catch (final IllegalArgumentException ignore) {
18630 }
18631 try {
18632 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18633 true, ba, new Matrix(1, 3), this);
18634 fail("IllegalArgumentException expected but not thrown");
18635 } catch (final IllegalArgumentException ignore) {
18636 }
18637 try {
18638 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18639 true, ba, new Matrix(3, 1), this);
18640 fail("IllegalArgumentException expected but not thrown");
18641 } catch (final IllegalArgumentException ignore) {
18642 }
18643 assertNull(calibrator);
18644 }
18645
18646 @Test
18647 public void testConstructor155() throws WrongSizeException {
18648 final Collection<StandardDeviationBodyKinematics> measurements =
18649 Collections.emptyList();
18650
18651 final Matrix ba = generateBa();
18652 final double[] bias = ba.getBuffer();
18653 final double biasX = ba.getElementAtIndex(0);
18654 final double biasY = ba.getElementAtIndex(1);
18655 final double biasZ = ba.getElementAtIndex(2);
18656
18657 final Matrix ma = generateMaCommonAxis();
18658 final double sx = ma.getElementAt(0, 0);
18659 final double sy = ma.getElementAt(1, 1);
18660 final double sz = ma.getElementAt(2, 2);
18661 final double mxy = ma.getElementAt(0, 1);
18662 final double mxz = ma.getElementAt(0, 2);
18663 final double myx = ma.getElementAt(1, 0);
18664 final double myz = ma.getElementAt(1, 2);
18665 final double mzx = ma.getElementAt(2, 0);
18666 final double mzy = ma.getElementAt(2, 1);
18667
18668 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18669 final double latitude = Math.toRadians(
18670 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18671 final double longitude = Math.toRadians(
18672 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18673 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18674 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18675 final NEDVelocity nedVelocity = new NEDVelocity();
18676 final ECEFPosition ecefPosition = new ECEFPosition();
18677 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18678 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18679 ecefPosition, ecefVelocity);
18680
18681 KnownPositionAccelerometerCalibrator calibrator =
18682 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
18683 true, ba, ma);
18684
18685
18686 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18687 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18688 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18689 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18690 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18691 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18692 final Acceleration bx2 = new Acceleration(0.0,
18693 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18694 calibrator.getInitialBiasXAsAcceleration(bx2);
18695 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18696 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18697 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18698 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18699 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18700 final Acceleration by2 = new Acceleration(0.0,
18701 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18702 calibrator.getInitialBiasYAsAcceleration(by2);
18703 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18704 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18705 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18706 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18707 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18708 final Acceleration bz2 = new Acceleration(0.0,
18709 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18710 calibrator.getInitialBiasZAsAcceleration(bz2);
18711 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18712 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18713 assertEquals(calibrator.getInitialSx(), sx, 0.0);
18714 assertEquals(calibrator.getInitialSy(), sy, 0.0);
18715 assertEquals(calibrator.getInitialSz(), sz, 0.0);
18716 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
18717 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
18718 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
18719 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
18720 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
18721 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
18722 final double[] bias1 = calibrator.getInitialBias();
18723 assertArrayEquals(bias1, bias, 0.0);
18724 final double[] bias2 = new double[3];
18725 calibrator.getInitialBias(bias2);
18726 assertArrayEquals(bias1, bias2, 0.0);
18727 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18728 assertEquals(b1, ba);
18729 final Matrix b2 = new Matrix(3, 1);
18730 calibrator.getInitialBiasAsMatrix(b2);
18731 assertEquals(b1, b2);
18732 final Matrix ma1 = new Matrix(3, 3);
18733 ma1.setSubmatrix(0, 0,
18734 2, 2,
18735 new double[]{ sx, myx, mzx,
18736 mxy, sy, mzy,
18737 mxz, myz, sz});
18738 assertEquals(calibrator.getInitialMa(), ma1);
18739 final Matrix ma2 = new Matrix(3, 3);
18740 calibrator.getInitialMa(ma2);
18741 assertEquals(ma1, ma2);
18742 assertSame(calibrator.getEcefPosition(), ecefPosition);
18743 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18744 final NEDPosition nedPosition1 = new NEDPosition();
18745 assertTrue(calibrator.getNedPosition(nedPosition1));
18746 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18747 assertSame(calibrator.getMeasurements(), measurements);
18748 assertTrue(calibrator.isCommonAxisUsed());
18749 assertNull(calibrator.getListener());
18750 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18751 assertFalse(calibrator.isReady());
18752 assertFalse(calibrator.isRunning());
18753 assertNull(calibrator.getEstimatedBiases());
18754 assertFalse(calibrator.getEstimatedBiases(null));
18755 assertNull(calibrator.getEstimatedBiasesAsMatrix());
18756 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18757 assertNull(calibrator.getEstimatedBiasFx());
18758 assertNull(calibrator.getEstimatedBiasFy());
18759 assertNull(calibrator.getEstimatedBiasFz());
18760 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18761 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18762 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18763 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18764 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18765 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18766 assertNull(calibrator.getEstimatedMa());
18767 assertNull(calibrator.getEstimatedSx());
18768 assertNull(calibrator.getEstimatedSy());
18769 assertNull(calibrator.getEstimatedSz());
18770 assertNull(calibrator.getEstimatedMxy());
18771 assertNull(calibrator.getEstimatedMxz());
18772 assertNull(calibrator.getEstimatedMyx());
18773 assertNull(calibrator.getEstimatedMyz());
18774 assertNull(calibrator.getEstimatedMzx());
18775 assertNull(calibrator.getEstimatedMzy());
18776 assertNull(calibrator.getEstimatedCovariance());
18777 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18778
18779
18780 calibrator = null;
18781 try {
18782 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18783 measurements, true,
18784 new Matrix(1, 1), ma);
18785 fail("IllegalArgumentException expected but not thrown");
18786 } catch (final IllegalArgumentException ignore) {
18787 }
18788 try {
18789 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18790 measurements, true,
18791 new Matrix(1, 3), ma);
18792 fail("IllegalArgumentException expected but not thrown");
18793 } catch (final IllegalArgumentException ignore) {
18794 }
18795 try {
18796 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18797 measurements, true,
18798 ba, new Matrix(1, 3));
18799 fail("IllegalArgumentException expected but not thrown");
18800 } catch (final IllegalArgumentException ignore) {
18801 }
18802 try {
18803 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18804 measurements, true,
18805 ba, new Matrix(3, 1));
18806 fail("IllegalArgumentException expected but not thrown");
18807 } catch (final IllegalArgumentException ignore) {
18808 }
18809 assertNull(calibrator);
18810 }
18811
18812 @Test
18813 public void testConstructor156() throws WrongSizeException {
18814 final Collection<StandardDeviationBodyKinematics> measurements =
18815 Collections.emptyList();
18816
18817 final Matrix ba = generateBa();
18818 final double[] bias = ba.getBuffer();
18819 final double biasX = ba.getElementAtIndex(0);
18820 final double biasY = ba.getElementAtIndex(1);
18821 final double biasZ = ba.getElementAtIndex(2);
18822
18823 final Matrix ma = generateMaCommonAxis();
18824 final double sx = ma.getElementAt(0, 0);
18825 final double sy = ma.getElementAt(1, 1);
18826 final double sz = ma.getElementAt(2, 2);
18827 final double mxy = ma.getElementAt(0, 1);
18828 final double mxz = ma.getElementAt(0, 2);
18829 final double myx = ma.getElementAt(1, 0);
18830 final double myz = ma.getElementAt(1, 2);
18831 final double mzx = ma.getElementAt(2, 0);
18832 final double mzy = ma.getElementAt(2, 1);
18833
18834 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18835 final double latitude = Math.toRadians(
18836 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18837 final double longitude = Math.toRadians(
18838 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18839 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18840 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18841 final NEDVelocity nedVelocity = new NEDVelocity();
18842 final ECEFPosition ecefPosition = new ECEFPosition();
18843 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18844 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18845 ecefPosition, ecefVelocity);
18846
18847 KnownPositionAccelerometerCalibrator calibrator =
18848 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
18849 true, ba, ma, this);
18850
18851
18852 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18853 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18854 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18855 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18856 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18857 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18858 final Acceleration bx2 = new Acceleration(0.0,
18859 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18860 calibrator.getInitialBiasXAsAcceleration(bx2);
18861 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18862 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18863 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18864 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18865 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18866 final Acceleration by2 = new Acceleration(0.0,
18867 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18868 calibrator.getInitialBiasYAsAcceleration(by2);
18869 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18870 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18871 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18872 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18873 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18874 final Acceleration bz2 = new Acceleration(0.0,
18875 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18876 calibrator.getInitialBiasZAsAcceleration(bz2);
18877 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18878 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18879 assertEquals(calibrator.getInitialSx(), sx, 0.0);
18880 assertEquals(calibrator.getInitialSy(), sy, 0.0);
18881 assertEquals(calibrator.getInitialSz(), sz, 0.0);
18882 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
18883 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
18884 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
18885 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
18886 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
18887 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
18888 final double[] bias1 = calibrator.getInitialBias();
18889 assertArrayEquals(bias1, bias, 0.0);
18890 final double[] bias2 = new double[3];
18891 calibrator.getInitialBias(bias2);
18892 assertArrayEquals(bias1, bias2, 0.0);
18893 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18894 assertEquals(b1, ba);
18895 final Matrix b2 = new Matrix(3, 1);
18896 calibrator.getInitialBiasAsMatrix(b2);
18897 assertEquals(b1, b2);
18898 final Matrix ma1 = new Matrix(3, 3);
18899 ma1.setSubmatrix(0, 0,
18900 2, 2,
18901 new double[]{ sx, myx, mzx,
18902 mxy, sy, mzy,
18903 mxz, myz, sz});
18904 assertEquals(calibrator.getInitialMa(), ma1);
18905 final Matrix ma2 = new Matrix(3, 3);
18906 calibrator.getInitialMa(ma2);
18907 assertEquals(ma1, ma2);
18908 assertSame(calibrator.getEcefPosition(), ecefPosition);
18909 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18910 final NEDPosition nedPosition1 = new NEDPosition();
18911 assertTrue(calibrator.getNedPosition(nedPosition1));
18912 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18913 assertSame(calibrator.getMeasurements(), measurements);
18914 assertTrue(calibrator.isCommonAxisUsed());
18915 assertSame(calibrator.getListener(), this);
18916 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18917 assertFalse(calibrator.isReady());
18918 assertFalse(calibrator.isRunning());
18919 assertNull(calibrator.getEstimatedBiases());
18920 assertFalse(calibrator.getEstimatedBiases(null));
18921 assertNull(calibrator.getEstimatedBiasesAsMatrix());
18922 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18923 assertNull(calibrator.getEstimatedBiasFx());
18924 assertNull(calibrator.getEstimatedBiasFy());
18925 assertNull(calibrator.getEstimatedBiasFz());
18926 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18927 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18928 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18929 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18930 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18931 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18932 assertNull(calibrator.getEstimatedMa());
18933 assertNull(calibrator.getEstimatedSx());
18934 assertNull(calibrator.getEstimatedSy());
18935 assertNull(calibrator.getEstimatedSz());
18936 assertNull(calibrator.getEstimatedMxy());
18937 assertNull(calibrator.getEstimatedMxz());
18938 assertNull(calibrator.getEstimatedMyx());
18939 assertNull(calibrator.getEstimatedMyz());
18940 assertNull(calibrator.getEstimatedMzx());
18941 assertNull(calibrator.getEstimatedMzy());
18942 assertNull(calibrator.getEstimatedCovariance());
18943 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18944
18945
18946 calibrator = null;
18947 try {
18948 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18949 measurements, true,
18950 new Matrix(1, 1), ma, this);
18951 fail("IllegalArgumentException expected but not thrown");
18952 } catch (final IllegalArgumentException ignore) {
18953 }
18954 try {
18955 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18956 measurements, true,
18957 new Matrix(1, 3), ma, this);
18958 fail("IllegalArgumentException expected but not thrown");
18959 } catch (final IllegalArgumentException ignore) {
18960 }
18961 try {
18962 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18963 measurements, true,
18964 ba, new Matrix(1, 3), this);
18965 fail("IllegalArgumentException expected but not thrown");
18966 } catch (final IllegalArgumentException ignore) {
18967 }
18968 try {
18969 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18970 measurements, true,
18971 ba, new Matrix(3, 1), this);
18972 fail("IllegalArgumentException expected but not thrown");
18973 } catch (final IllegalArgumentException ignore) {
18974 }
18975 assertNull(calibrator);
18976 }
18977
18978 @Test
18979 public void testConstructor157() throws WrongSizeException {
18980 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18981 final double latitude = Math.toRadians(
18982 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18983 final double longitude = Math.toRadians(
18984 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18985 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18986 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18987 final NEDVelocity nedVelocity = new NEDVelocity();
18988 final ECEFPosition ecefPosition = new ECEFPosition();
18989 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18990 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18991 ecefPosition, ecefVelocity);
18992
18993 final KnownPositionAccelerometerCalibrator calibrator =
18994 new KnownPositionAccelerometerCalibrator(nedPosition);
18995
18996
18997 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
18998 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
18999 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
19000 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19001 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
19002 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19003 final Acceleration bx2 = new Acceleration(0.0,
19004 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19005 calibrator.getInitialBiasXAsAcceleration(bx2);
19006 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
19007 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19008 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19009 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
19010 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19011 final Acceleration by2 = new Acceleration(0.0,
19012 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19013 calibrator.getInitialBiasYAsAcceleration(by2);
19014 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
19015 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19016 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19017 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
19018 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19019 final Acceleration bz2 = new Acceleration(0.0,
19020 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19021 calibrator.getInitialBiasZAsAcceleration(bz2);
19022 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
19023 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19024 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19025 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19026 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19027 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19028 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19029 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19030 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19031 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19032 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19033 final double[] bias1 = calibrator.getInitialBias();
19034 assertArrayEquals(bias1, new double[3], 0.0);
19035 final double[] bias2 = new double[3];
19036 calibrator.getInitialBias(bias2);
19037 assertArrayEquals(bias1, bias2, 0.0);
19038 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19039 assertEquals(b1, new Matrix(3, 1));
19040 final Matrix b2 = new Matrix(3, 1);
19041 calibrator.getInitialBiasAsMatrix(b2);
19042 assertEquals(b1, b2);
19043 final Matrix ma1 = calibrator.getInitialMa();
19044 assertEquals(ma1, new Matrix(3, 3));
19045 final Matrix ma2 = new Matrix(3, 3);
19046 calibrator.getInitialMa(ma2);
19047 assertEquals(ma1, ma2);
19048 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
19049 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19050 final NEDPosition nedPosition1 = new NEDPosition();
19051 assertTrue(calibrator.getNedPosition(nedPosition1));
19052 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19053 assertNull(calibrator.getMeasurements());
19054 assertFalse(calibrator.isCommonAxisUsed());
19055 assertNull(calibrator.getListener());
19056 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
19057 assertFalse(calibrator.isReady());
19058 assertFalse(calibrator.isRunning());
19059 assertNull(calibrator.getEstimatedBiases());
19060 assertFalse(calibrator.getEstimatedBiases(null));
19061 assertNull(calibrator.getEstimatedBiasesAsMatrix());
19062 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19063 assertNull(calibrator.getEstimatedBiasFx());
19064 assertNull(calibrator.getEstimatedBiasFy());
19065 assertNull(calibrator.getEstimatedBiasFz());
19066 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19067 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19068 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19069 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19070 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19071 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19072 assertNull(calibrator.getEstimatedMa());
19073 assertNull(calibrator.getEstimatedSx());
19074 assertNull(calibrator.getEstimatedSy());
19075 assertNull(calibrator.getEstimatedSz());
19076 assertNull(calibrator.getEstimatedMxy());
19077 assertNull(calibrator.getEstimatedMxz());
19078 assertNull(calibrator.getEstimatedMyx());
19079 assertNull(calibrator.getEstimatedMyz());
19080 assertNull(calibrator.getEstimatedMzx());
19081 assertNull(calibrator.getEstimatedMzy());
19082 assertNull(calibrator.getEstimatedCovariance());
19083 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19084 }
19085
19086 @Test
19087 public void testConstructor158() throws WrongSizeException {
19088 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19089 final double latitude = Math.toRadians(
19090 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19091 final double longitude = Math.toRadians(
19092 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19093 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19094 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19095 final NEDVelocity nedVelocity = new NEDVelocity();
19096 final ECEFPosition ecefPosition = new ECEFPosition();
19097 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19098 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19099 ecefPosition, ecefVelocity);
19100
19101 final KnownPositionAccelerometerCalibrator calibrator =
19102 new KnownPositionAccelerometerCalibrator(nedPosition, this);
19103
19104
19105 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
19106 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
19107 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
19108 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19109 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
19110 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19111 final Acceleration bx2 = new Acceleration(0.0,
19112 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19113 calibrator.getInitialBiasXAsAcceleration(bx2);
19114 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
19115 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19116 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19117 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
19118 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19119 final Acceleration by2 = new Acceleration(0.0,
19120 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19121 calibrator.getInitialBiasYAsAcceleration(by2);
19122 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
19123 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19124 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19125 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
19126 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19127 final Acceleration bz2 = new Acceleration(0.0,
19128 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19129 calibrator.getInitialBiasZAsAcceleration(bz2);
19130 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
19131 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19132 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19133 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19134 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19135 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19136 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19137 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19138 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19139 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19140 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19141 final double[] bias1 = calibrator.getInitialBias();
19142 assertArrayEquals(bias1, new double[3], 0.0);
19143 final double[] bias2 = new double[3];
19144 calibrator.getInitialBias(bias2);
19145 assertArrayEquals(bias1, bias2, 0.0);
19146 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19147 assertEquals(b1, new Matrix(3, 1));
19148 final Matrix b2 = new Matrix(3, 1);
19149 calibrator.getInitialBiasAsMatrix(b2);
19150 assertEquals(b1, b2);
19151 final Matrix ma1 = calibrator.getInitialMa();
19152 assertEquals(ma1, new Matrix(3, 3));
19153 final Matrix ma2 = new Matrix(3, 3);
19154 calibrator.getInitialMa(ma2);
19155 assertEquals(ma1, ma2);
19156 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
19157 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19158 final NEDPosition nedPosition1 = new NEDPosition();
19159 assertTrue(calibrator.getNedPosition(nedPosition1));
19160 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19161 assertNull(calibrator.getMeasurements());
19162 assertFalse(calibrator.isCommonAxisUsed());
19163 assertSame(calibrator.getListener(), this);
19164 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
19165 assertFalse(calibrator.isReady());
19166 assertFalse(calibrator.isRunning());
19167 assertNull(calibrator.getEstimatedBiases());
19168 assertFalse(calibrator.getEstimatedBiases(null));
19169 assertNull(calibrator.getEstimatedBiasesAsMatrix());
19170 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19171 assertNull(calibrator.getEstimatedBiasFx());
19172 assertNull(calibrator.getEstimatedBiasFy());
19173 assertNull(calibrator.getEstimatedBiasFz());
19174 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19175 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19176 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19177 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19178 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19179 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19180 assertNull(calibrator.getEstimatedMa());
19181 assertNull(calibrator.getEstimatedSx());
19182 assertNull(calibrator.getEstimatedSy());
19183 assertNull(calibrator.getEstimatedSz());
19184 assertNull(calibrator.getEstimatedMxy());
19185 assertNull(calibrator.getEstimatedMxz());
19186 assertNull(calibrator.getEstimatedMyx());
19187 assertNull(calibrator.getEstimatedMyz());
19188 assertNull(calibrator.getEstimatedMzx());
19189 assertNull(calibrator.getEstimatedMzy());
19190 assertNull(calibrator.getEstimatedCovariance());
19191 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19192 }
19193
19194 @Test
19195 public void testConstructor159() throws WrongSizeException {
19196 final Collection<StandardDeviationBodyKinematics> measurements =
19197 Collections.emptyList();
19198
19199 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19200 final double latitude = Math.toRadians(
19201 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19202 final double longitude = Math.toRadians(
19203 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19204 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19205 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19206 final NEDVelocity nedVelocity = new NEDVelocity();
19207 final ECEFPosition ecefPosition = new ECEFPosition();
19208 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19209 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19210 ecefPosition, ecefVelocity);
19211
19212 final KnownPositionAccelerometerCalibrator calibrator =
19213 new KnownPositionAccelerometerCalibrator(nedPosition, measurements);
19214
19215
19216 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
19217 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
19218 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
19219 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19220 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
19221 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19222 final Acceleration bx2 = new Acceleration(0.0,
19223 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19224 calibrator.getInitialBiasXAsAcceleration(bx2);
19225 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
19226 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19227 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19228 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
19229 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19230 final Acceleration by2 = new Acceleration(0.0,
19231 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19232 calibrator.getInitialBiasYAsAcceleration(by2);
19233 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
19234 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19235 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19236 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
19237 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19238 final Acceleration bz2 = new Acceleration(0.0,
19239 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19240 calibrator.getInitialBiasZAsAcceleration(bz2);
19241 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
19242 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19243 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19244 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19245 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19246 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19247 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19248 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19249 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19250 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19251 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19252 final double[] bias1 = calibrator.getInitialBias();
19253 assertArrayEquals(bias1, new double[3], 0.0);
19254 final double[] bias2 = new double[3];
19255 calibrator.getInitialBias(bias2);
19256 assertArrayEquals(bias1, bias2, 0.0);
19257 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19258 assertEquals(b1, new Matrix(3, 1));
19259 final Matrix b2 = new Matrix(3, 1);
19260 calibrator.getInitialBiasAsMatrix(b2);
19261 assertEquals(b1, b2);
19262 final Matrix ma1 = calibrator.getInitialMa();
19263 assertEquals(ma1, new Matrix(3, 3));
19264 final Matrix ma2 = new Matrix(3, 3);
19265 calibrator.getInitialMa(ma2);
19266 assertEquals(ma1, ma2);
19267 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
19268 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19269 final NEDPosition nedPosition1 = new NEDPosition();
19270 assertTrue(calibrator.getNedPosition(nedPosition1));
19271 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19272 assertSame(calibrator.getMeasurements(), measurements);
19273 assertFalse(calibrator.isCommonAxisUsed());
19274 assertNull(calibrator.getListener());
19275 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
19276 assertFalse(calibrator.isReady());
19277 assertFalse(calibrator.isRunning());
19278 assertNull(calibrator.getEstimatedBiases());
19279 assertFalse(calibrator.getEstimatedBiases(null));
19280 assertNull(calibrator.getEstimatedBiasesAsMatrix());
19281 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19282 assertNull(calibrator.getEstimatedBiasFx());
19283 assertNull(calibrator.getEstimatedBiasFy());
19284 assertNull(calibrator.getEstimatedBiasFz());
19285 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19286 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19287 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19288 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19289 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19290 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19291 assertNull(calibrator.getEstimatedMa());
19292 assertNull(calibrator.getEstimatedSx());
19293 assertNull(calibrator.getEstimatedSy());
19294 assertNull(calibrator.getEstimatedSz());
19295 assertNull(calibrator.getEstimatedMxy());
19296 assertNull(calibrator.getEstimatedMxz());
19297 assertNull(calibrator.getEstimatedMyx());
19298 assertNull(calibrator.getEstimatedMyz());
19299 assertNull(calibrator.getEstimatedMzx());
19300 assertNull(calibrator.getEstimatedMzy());
19301 assertNull(calibrator.getEstimatedCovariance());
19302 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19303 }
19304
19305 @Test
19306 public void testConstructor160() throws WrongSizeException {
19307 final Collection<StandardDeviationBodyKinematics> measurements =
19308 Collections.emptyList();
19309
19310 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19311 final double latitude = Math.toRadians(
19312 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19313 final double longitude = Math.toRadians(
19314 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19315 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19316 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19317 final NEDVelocity nedVelocity = new NEDVelocity();
19318 final ECEFPosition ecefPosition = new ECEFPosition();
19319 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19320 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19321 ecefPosition, ecefVelocity);
19322
19323 final KnownPositionAccelerometerCalibrator calibrator =
19324 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
19325 this);
19326
19327
19328 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
19329 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
19330 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
19331 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19332 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
19333 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19334 final Acceleration bx2 = new Acceleration(0.0,
19335 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19336 calibrator.getInitialBiasXAsAcceleration(bx2);
19337 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
19338 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19339 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19340 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
19341 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19342 final Acceleration by2 = new Acceleration(0.0,
19343 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19344 calibrator.getInitialBiasYAsAcceleration(by2);
19345 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
19346 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19347 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19348 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
19349 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19350 final Acceleration bz2 = new Acceleration(0.0,
19351 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19352 calibrator.getInitialBiasZAsAcceleration(bz2);
19353 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
19354 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19355 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19356 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19357 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19358 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19359 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19360 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19361 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19362 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19363 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19364 final double[] bias1 = calibrator.getInitialBias();
19365 assertArrayEquals(bias1, new double[3], 0.0);
19366 final double[] bias2 = new double[3];
19367 calibrator.getInitialBias(bias2);
19368 assertArrayEquals(bias1, bias2, 0.0);
19369 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19370 assertEquals(b1, new Matrix(3, 1));
19371 final Matrix b2 = new Matrix(3, 1);
19372 calibrator.getInitialBiasAsMatrix(b2);
19373 assertEquals(b1, b2);
19374 final Matrix ma1 = calibrator.getInitialMa();
19375 assertEquals(ma1, new Matrix(3, 3));
19376 final Matrix ma2 = new Matrix(3, 3);
19377 calibrator.getInitialMa(ma2);
19378 assertEquals(ma1, ma2);
19379 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
19380 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19381 final NEDPosition nedPosition1 = new NEDPosition();
19382 assertTrue(calibrator.getNedPosition(nedPosition1));
19383 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19384 assertSame(calibrator.getMeasurements(), measurements);
19385 assertFalse(calibrator.isCommonAxisUsed());
19386 assertSame(calibrator.getListener(), this);
19387 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
19388 assertFalse(calibrator.isReady());
19389 assertFalse(calibrator.isRunning());
19390 assertNull(calibrator.getEstimatedBiases());
19391 assertFalse(calibrator.getEstimatedBiases(null));
19392 assertNull(calibrator.getEstimatedBiasesAsMatrix());
19393 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19394 assertNull(calibrator.getEstimatedBiasFx());
19395 assertNull(calibrator.getEstimatedBiasFy());
19396 assertNull(calibrator.getEstimatedBiasFz());
19397 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19398 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19399 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19400 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19401 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19402 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19403 assertNull(calibrator.getEstimatedMa());
19404 assertNull(calibrator.getEstimatedSx());
19405 assertNull(calibrator.getEstimatedSy());
19406 assertNull(calibrator.getEstimatedSz());
19407 assertNull(calibrator.getEstimatedMxy());
19408 assertNull(calibrator.getEstimatedMxz());
19409 assertNull(calibrator.getEstimatedMyx());
19410 assertNull(calibrator.getEstimatedMyz());
19411 assertNull(calibrator.getEstimatedMzx());
19412 assertNull(calibrator.getEstimatedMzy());
19413 assertNull(calibrator.getEstimatedCovariance());
19414 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19415 }
19416
19417 @Test
19418 public void testConstructor161() throws WrongSizeException {
19419 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19420 final double latitude = Math.toRadians(
19421 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19422 final double longitude = Math.toRadians(
19423 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19424 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19425 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19426 final NEDVelocity nedVelocity = new NEDVelocity();
19427 final ECEFPosition ecefPosition = new ECEFPosition();
19428 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19429 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19430 ecefPosition, ecefVelocity);
19431
19432 final KnownPositionAccelerometerCalibrator calibrator =
19433 new KnownPositionAccelerometerCalibrator(nedPosition,
19434 true);
19435
19436
19437 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
19438 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
19439 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
19440 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19441 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
19442 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19443 final Acceleration bx2 = new Acceleration(0.0,
19444 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19445 calibrator.getInitialBiasXAsAcceleration(bx2);
19446 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
19447 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19448 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19449 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
19450 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19451 final Acceleration by2 = new Acceleration(0.0,
19452 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19453 calibrator.getInitialBiasYAsAcceleration(by2);
19454 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
19455 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19456 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19457 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
19458 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19459 final Acceleration bz2 = new Acceleration(0.0,
19460 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19461 calibrator.getInitialBiasZAsAcceleration(bz2);
19462 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
19463 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19464 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19465 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19466 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19467 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19468 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19469 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19470 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19471 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19472 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19473 final double[] bias1 = calibrator.getInitialBias();
19474 assertArrayEquals(bias1, new double[3], 0.0);
19475 final double[] bias2 = new double[3];
19476 calibrator.getInitialBias(bias2);
19477 assertArrayEquals(bias1, bias2, 0.0);
19478 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19479 assertEquals(b1, new Matrix(3, 1));
19480 final Matrix b2 = new Matrix(3, 1);
19481 calibrator.getInitialBiasAsMatrix(b2);
19482 assertEquals(b1, b2);
19483 final Matrix ma1 = calibrator.getInitialMa();
19484 assertEquals(ma1, new Matrix(3, 3));
19485 final Matrix ma2 = new Matrix(3, 3);
19486 calibrator.getInitialMa(ma2);
19487 assertEquals(ma1, ma2);
19488 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
19489 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19490 final NEDPosition nedPosition1 = new NEDPosition();
19491 assertTrue(calibrator.getNedPosition(nedPosition1));
19492 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19493 assertNull(calibrator.getMeasurements());
19494 assertTrue(calibrator.isCommonAxisUsed());
19495 assertNull(calibrator.getListener());
19496 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19497 assertFalse(calibrator.isReady());
19498 assertFalse(calibrator.isRunning());
19499 assertNull(calibrator.getEstimatedBiases());
19500 assertFalse(calibrator.getEstimatedBiases(null));
19501 assertNull(calibrator.getEstimatedBiasesAsMatrix());
19502 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19503 assertNull(calibrator.getEstimatedBiasFx());
19504 assertNull(calibrator.getEstimatedBiasFy());
19505 assertNull(calibrator.getEstimatedBiasFz());
19506 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19507 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19508 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19509 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19510 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19511 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19512 assertNull(calibrator.getEstimatedMa());
19513 assertNull(calibrator.getEstimatedSx());
19514 assertNull(calibrator.getEstimatedSy());
19515 assertNull(calibrator.getEstimatedSz());
19516 assertNull(calibrator.getEstimatedMxy());
19517 assertNull(calibrator.getEstimatedMxz());
19518 assertNull(calibrator.getEstimatedMyx());
19519 assertNull(calibrator.getEstimatedMyz());
19520 assertNull(calibrator.getEstimatedMzx());
19521 assertNull(calibrator.getEstimatedMzy());
19522 assertNull(calibrator.getEstimatedCovariance());
19523 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19524 }
19525
19526 @Test
19527 public void testConstructor162() throws WrongSizeException {
19528 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19529 final double latitude = Math.toRadians(
19530 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19531 final double longitude = Math.toRadians(
19532 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19533 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19534 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19535 final NEDVelocity nedVelocity = new NEDVelocity();
19536 final ECEFPosition ecefPosition = new ECEFPosition();
19537 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19538 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19539 ecefPosition, ecefVelocity);
19540
19541 final KnownPositionAccelerometerCalibrator calibrator =
19542 new KnownPositionAccelerometerCalibrator(nedPosition,
19543 true, this);
19544
19545
19546 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
19547 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
19548 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
19549 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19550 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
19551 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19552 final Acceleration bx2 = new Acceleration(0.0,
19553 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19554 calibrator.getInitialBiasXAsAcceleration(bx2);
19555 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
19556 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19557 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19558 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
19559 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19560 final Acceleration by2 = new Acceleration(0.0,
19561 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19562 calibrator.getInitialBiasYAsAcceleration(by2);
19563 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
19564 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19565 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19566 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
19567 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19568 final Acceleration bz2 = new Acceleration(0.0,
19569 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19570 calibrator.getInitialBiasZAsAcceleration(bz2);
19571 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
19572 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19573 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19574 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19575 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19576 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19577 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19578 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19579 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19580 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19581 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19582 final double[] bias1 = calibrator.getInitialBias();
19583 assertArrayEquals(bias1, new double[3], 0.0);
19584 final double[] bias2 = new double[3];
19585 calibrator.getInitialBias(bias2);
19586 assertArrayEquals(bias1, bias2, 0.0);
19587 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19588 assertEquals(b1, new Matrix(3, 1));
19589 final Matrix b2 = new Matrix(3, 1);
19590 calibrator.getInitialBiasAsMatrix(b2);
19591 assertEquals(b1, b2);
19592 final Matrix ma1 = calibrator.getInitialMa();
19593 assertEquals(ma1, new Matrix(3, 3));
19594 final Matrix ma2 = new Matrix(3, 3);
19595 calibrator.getInitialMa(ma2);
19596 assertEquals(ma1, ma2);
19597 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
19598 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19599 final NEDPosition nedPosition1 = new NEDPosition();
19600 assertTrue(calibrator.getNedPosition(nedPosition1));
19601 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19602 assertNull(calibrator.getMeasurements());
19603 assertTrue(calibrator.isCommonAxisUsed());
19604 assertSame(calibrator.getListener(), this);
19605 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19606 assertFalse(calibrator.isReady());
19607 assertFalse(calibrator.isRunning());
19608 assertNull(calibrator.getEstimatedBiases());
19609 assertFalse(calibrator.getEstimatedBiases(null));
19610 assertNull(calibrator.getEstimatedBiasesAsMatrix());
19611 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19612 assertNull(calibrator.getEstimatedBiasFx());
19613 assertNull(calibrator.getEstimatedBiasFy());
19614 assertNull(calibrator.getEstimatedBiasFz());
19615 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19616 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19617 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19618 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19619 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19620 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19621 assertNull(calibrator.getEstimatedMa());
19622 assertNull(calibrator.getEstimatedSx());
19623 assertNull(calibrator.getEstimatedSy());
19624 assertNull(calibrator.getEstimatedSz());
19625 assertNull(calibrator.getEstimatedMxy());
19626 assertNull(calibrator.getEstimatedMxz());
19627 assertNull(calibrator.getEstimatedMyx());
19628 assertNull(calibrator.getEstimatedMyz());
19629 assertNull(calibrator.getEstimatedMzx());
19630 assertNull(calibrator.getEstimatedMzy());
19631 assertNull(calibrator.getEstimatedCovariance());
19632 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19633 }
19634
19635 @Test
19636 public void testConstructor163() throws WrongSizeException {
19637 final Collection<StandardDeviationBodyKinematics> measurements =
19638 Collections.emptyList();
19639 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19640 final double latitude = Math.toRadians(
19641 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19642 final double longitude = Math.toRadians(
19643 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19644 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19645 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19646 final NEDVelocity nedVelocity = new NEDVelocity();
19647 final ECEFPosition ecefPosition = new ECEFPosition();
19648 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19649 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19650 ecefPosition, ecefVelocity);
19651
19652 final KnownPositionAccelerometerCalibrator calibrator =
19653 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
19654 true);
19655
19656
19657 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
19658 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
19659 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
19660 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19661 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
19662 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19663 final Acceleration bx2 = new Acceleration(0.0,
19664 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19665 calibrator.getInitialBiasXAsAcceleration(bx2);
19666 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
19667 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19668 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19669 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
19670 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19671 final Acceleration by2 = new Acceleration(0.0,
19672 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19673 calibrator.getInitialBiasYAsAcceleration(by2);
19674 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
19675 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19676 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19677 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
19678 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19679 final Acceleration bz2 = new Acceleration(0.0,
19680 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19681 calibrator.getInitialBiasZAsAcceleration(bz2);
19682 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
19683 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19684 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19685 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19686 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19687 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19688 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19689 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19690 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19691 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19692 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19693 final double[] bias1 = calibrator.getInitialBias();
19694 assertArrayEquals(bias1, new double[3], 0.0);
19695 final double[] bias2 = new double[3];
19696 calibrator.getInitialBias(bias2);
19697 assertArrayEquals(bias1, bias2, 0.0);
19698 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19699 assertEquals(b1, new Matrix(3, 1));
19700 final Matrix b2 = new Matrix(3, 1);
19701 calibrator.getInitialBiasAsMatrix(b2);
19702 assertEquals(b1, b2);
19703 final Matrix ma1 = calibrator.getInitialMa();
19704 assertEquals(ma1, new Matrix(3, 3));
19705 final Matrix ma2 = new Matrix(3, 3);
19706 calibrator.getInitialMa(ma2);
19707 assertEquals(ma1, ma2);
19708 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
19709 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19710 final NEDPosition nedPosition1 = new NEDPosition();
19711 assertTrue(calibrator.getNedPosition(nedPosition1));
19712 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19713 assertSame(calibrator.getMeasurements(), measurements);
19714 assertTrue(calibrator.isCommonAxisUsed());
19715 assertNull(calibrator.getListener());
19716 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19717 assertFalse(calibrator.isReady());
19718 assertFalse(calibrator.isRunning());
19719 assertNull(calibrator.getEstimatedBiases());
19720 assertFalse(calibrator.getEstimatedBiases(null));
19721 assertNull(calibrator.getEstimatedBiasesAsMatrix());
19722 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19723 assertNull(calibrator.getEstimatedBiasFx());
19724 assertNull(calibrator.getEstimatedBiasFy());
19725 assertNull(calibrator.getEstimatedBiasFz());
19726 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19727 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19728 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19729 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19730 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19731 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19732 assertNull(calibrator.getEstimatedMa());
19733 assertNull(calibrator.getEstimatedSx());
19734 assertNull(calibrator.getEstimatedSy());
19735 assertNull(calibrator.getEstimatedSz());
19736 assertNull(calibrator.getEstimatedMxy());
19737 assertNull(calibrator.getEstimatedMxz());
19738 assertNull(calibrator.getEstimatedMyx());
19739 assertNull(calibrator.getEstimatedMyz());
19740 assertNull(calibrator.getEstimatedMzx());
19741 assertNull(calibrator.getEstimatedMzy());
19742 assertNull(calibrator.getEstimatedCovariance());
19743 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19744 }
19745
19746 @Test
19747 public void testConstructor164() throws WrongSizeException {
19748 final Collection<StandardDeviationBodyKinematics> measurements =
19749 Collections.emptyList();
19750 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19751 final double latitude = Math.toRadians(
19752 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19753 final double longitude = Math.toRadians(
19754 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19755 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19756 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19757 final NEDVelocity nedVelocity = new NEDVelocity();
19758 final ECEFPosition ecefPosition = new ECEFPosition();
19759 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19760 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19761 ecefPosition, ecefVelocity);
19762
19763 final KnownPositionAccelerometerCalibrator calibrator =
19764 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
19765 true, this);
19766
19767
19768 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
19769 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
19770 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
19771 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19772 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
19773 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19774 final Acceleration bx2 = new Acceleration(0.0,
19775 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19776 calibrator.getInitialBiasXAsAcceleration(bx2);
19777 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
19778 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19779 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19780 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
19781 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19782 final Acceleration by2 = new Acceleration(0.0,
19783 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19784 calibrator.getInitialBiasYAsAcceleration(by2);
19785 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
19786 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19787 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19788 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
19789 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19790 final Acceleration bz2 = new Acceleration(0.0,
19791 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19792 calibrator.getInitialBiasZAsAcceleration(bz2);
19793 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
19794 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19795 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19796 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19797 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19798 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19799 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19800 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19801 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19802 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19803 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19804 final double[] bias1 = calibrator.getInitialBias();
19805 assertArrayEquals(bias1, new double[3], 0.0);
19806 final double[] bias2 = new double[3];
19807 calibrator.getInitialBias(bias2);
19808 assertArrayEquals(bias1, bias2, 0.0);
19809 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19810 assertEquals(b1, new Matrix(3, 1));
19811 final Matrix b2 = new Matrix(3, 1);
19812 calibrator.getInitialBiasAsMatrix(b2);
19813 assertEquals(b1, b2);
19814 final Matrix ma1 = calibrator.getInitialMa();
19815 assertEquals(ma1, new Matrix(3, 3));
19816 final Matrix ma2 = new Matrix(3, 3);
19817 calibrator.getInitialMa(ma2);
19818 assertEquals(ma1, ma2);
19819 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
19820 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19821 final NEDPosition nedPosition1 = new NEDPosition();
19822 assertTrue(calibrator.getNedPosition(nedPosition1));
19823 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19824 assertSame(calibrator.getMeasurements(), measurements);
19825 assertTrue(calibrator.isCommonAxisUsed());
19826 assertSame(calibrator.getListener(), this);
19827 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19828 assertFalse(calibrator.isReady());
19829 assertFalse(calibrator.isRunning());
19830 assertNull(calibrator.getEstimatedBiases());
19831 assertFalse(calibrator.getEstimatedBiases(null));
19832 assertNull(calibrator.getEstimatedBiasesAsMatrix());
19833 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19834 assertNull(calibrator.getEstimatedBiasFx());
19835 assertNull(calibrator.getEstimatedBiasFy());
19836 assertNull(calibrator.getEstimatedBiasFz());
19837 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19838 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19839 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19840 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19841 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19842 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19843 assertNull(calibrator.getEstimatedMa());
19844 assertNull(calibrator.getEstimatedSx());
19845 assertNull(calibrator.getEstimatedSy());
19846 assertNull(calibrator.getEstimatedSz());
19847 assertNull(calibrator.getEstimatedMxy());
19848 assertNull(calibrator.getEstimatedMxz());
19849 assertNull(calibrator.getEstimatedMyx());
19850 assertNull(calibrator.getEstimatedMyz());
19851 assertNull(calibrator.getEstimatedMzx());
19852 assertNull(calibrator.getEstimatedMzy());
19853 assertNull(calibrator.getEstimatedCovariance());
19854 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19855 }
19856
19857 @Test
19858 public void testConstructor165() throws WrongSizeException {
19859 final Matrix ba = generateBa();
19860 final double biasX = ba.getElementAtIndex(0);
19861 final double biasY = ba.getElementAtIndex(1);
19862 final double biasZ = ba.getElementAtIndex(2);
19863
19864 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19865 final double latitude = Math.toRadians(
19866 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19867 final double longitude = Math.toRadians(
19868 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19869 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19870 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19871 final NEDVelocity nedVelocity = new NEDVelocity();
19872 final ECEFPosition ecefPosition = new ECEFPosition();
19873 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19874 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19875 ecefPosition, ecefVelocity);
19876
19877 final KnownPositionAccelerometerCalibrator calibrator =
19878 new KnownPositionAccelerometerCalibrator(nedPosition,
19879 biasX, biasY, biasZ);
19880
19881
19882 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19883 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19884 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19885 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19886 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19887 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19888 final Acceleration bx2 = new Acceleration(0.0,
19889 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19890 calibrator.getInitialBiasXAsAcceleration(bx2);
19891 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19892 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19893 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19894 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19895 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19896 final Acceleration by2 = new Acceleration(0.0,
19897 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19898 calibrator.getInitialBiasYAsAcceleration(by2);
19899 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19900 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19901 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19902 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19903 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19904 final Acceleration bz2 = new Acceleration(0.0,
19905 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19906 calibrator.getInitialBiasZAsAcceleration(bz2);
19907 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19908 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19909 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19910 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19911 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19912 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19913 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19914 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19915 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19916 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19917 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19918 final double[] bias1 = calibrator.getInitialBias();
19919 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
19920 final double[] bias2 = new double[3];
19921 calibrator.getInitialBias(bias2);
19922 assertArrayEquals(bias1, bias2, 0.0);
19923 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19924 assertEquals(b1, ba);
19925 final Matrix b2 = new Matrix(3, 1);
19926 calibrator.getInitialBiasAsMatrix(b2);
19927 assertEquals(b1, b2);
19928 final Matrix ma1 = calibrator.getInitialMa();
19929 assertEquals(ma1, new Matrix(3, 3));
19930 final Matrix ma2 = new Matrix(3, 3);
19931 calibrator.getInitialMa(ma2);
19932 assertEquals(ma1, ma2);
19933 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
19934 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19935 final NEDPosition nedPosition1 = new NEDPosition();
19936 assertTrue(calibrator.getNedPosition(nedPosition1));
19937 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19938 assertNull(calibrator.getMeasurements());
19939 assertFalse(calibrator.isCommonAxisUsed());
19940 assertNull(calibrator.getListener());
19941 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
19942 assertFalse(calibrator.isReady());
19943 assertFalse(calibrator.isRunning());
19944 assertNull(calibrator.getEstimatedBiases());
19945 assertFalse(calibrator.getEstimatedBiases(null));
19946 assertNull(calibrator.getEstimatedBiasesAsMatrix());
19947 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19948 assertNull(calibrator.getEstimatedBiasFx());
19949 assertNull(calibrator.getEstimatedBiasFy());
19950 assertNull(calibrator.getEstimatedBiasFz());
19951 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19952 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19953 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19954 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19955 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19956 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19957 assertNull(calibrator.getEstimatedMa());
19958 assertNull(calibrator.getEstimatedSx());
19959 assertNull(calibrator.getEstimatedSy());
19960 assertNull(calibrator.getEstimatedSz());
19961 assertNull(calibrator.getEstimatedMxy());
19962 assertNull(calibrator.getEstimatedMxz());
19963 assertNull(calibrator.getEstimatedMyx());
19964 assertNull(calibrator.getEstimatedMyz());
19965 assertNull(calibrator.getEstimatedMzx());
19966 assertNull(calibrator.getEstimatedMzy());
19967 assertNull(calibrator.getEstimatedCovariance());
19968 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19969 }
19970
19971 @Test
19972 public void testConstructor166() throws WrongSizeException {
19973 final Matrix ba = generateBa();
19974 final double biasX = ba.getElementAtIndex(0);
19975 final double biasY = ba.getElementAtIndex(1);
19976 final double biasZ = ba.getElementAtIndex(2);
19977
19978 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19979 final double latitude = Math.toRadians(
19980 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19981 final double longitude = Math.toRadians(
19982 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19983 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19984 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19985 final NEDVelocity nedVelocity = new NEDVelocity();
19986 final ECEFPosition ecefPosition = new ECEFPosition();
19987 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19988 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19989 ecefPosition, ecefVelocity);
19990
19991 final KnownPositionAccelerometerCalibrator calibrator =
19992 new KnownPositionAccelerometerCalibrator(nedPosition,
19993 biasX, biasY, biasZ, this);
19994
19995
19996 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19997 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19998 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19999 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20000 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20001 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20002 final Acceleration bx2 = new Acceleration(0.0,
20003 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20004 calibrator.getInitialBiasXAsAcceleration(bx2);
20005 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20006 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20007 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20008 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20009 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20010 final Acceleration by2 = new Acceleration(0.0,
20011 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20012 calibrator.getInitialBiasYAsAcceleration(by2);
20013 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20014 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20015 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20016 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20017 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20018 final Acceleration bz2 = new Acceleration(0.0,
20019 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20020 calibrator.getInitialBiasZAsAcceleration(bz2);
20021 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20022 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20023 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20024 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20025 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20026 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20027 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20028 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20029 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20030 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20031 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20032 final double[] bias1 = calibrator.getInitialBias();
20033 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
20034 final double[] bias2 = new double[3];
20035 calibrator.getInitialBias(bias2);
20036 assertArrayEquals(bias1, bias2, 0.0);
20037 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20038 assertEquals(b1, ba);
20039 final Matrix b2 = new Matrix(3, 1);
20040 calibrator.getInitialBiasAsMatrix(b2);
20041 assertEquals(b1, b2);
20042 final Matrix ma1 = calibrator.getInitialMa();
20043 assertEquals(ma1, new Matrix(3, 3));
20044 final Matrix ma2 = new Matrix(3, 3);
20045 calibrator.getInitialMa(ma2);
20046 assertEquals(ma1, ma2);
20047 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20048 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20049 final NEDPosition nedPosition1 = new NEDPosition();
20050 assertTrue(calibrator.getNedPosition(nedPosition1));
20051 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20052 assertNull(calibrator.getMeasurements());
20053 assertFalse(calibrator.isCommonAxisUsed());
20054 assertSame(calibrator.getListener(), this);
20055 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20056 assertFalse(calibrator.isReady());
20057 assertFalse(calibrator.isRunning());
20058 assertNull(calibrator.getEstimatedBiases());
20059 assertFalse(calibrator.getEstimatedBiases(null));
20060 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20061 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20062 assertNull(calibrator.getEstimatedBiasFx());
20063 assertNull(calibrator.getEstimatedBiasFy());
20064 assertNull(calibrator.getEstimatedBiasFz());
20065 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20066 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20067 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20068 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20069 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20070 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20071 assertNull(calibrator.getEstimatedMa());
20072 assertNull(calibrator.getEstimatedSx());
20073 assertNull(calibrator.getEstimatedSy());
20074 assertNull(calibrator.getEstimatedSz());
20075 assertNull(calibrator.getEstimatedMxy());
20076 assertNull(calibrator.getEstimatedMxz());
20077 assertNull(calibrator.getEstimatedMyx());
20078 assertNull(calibrator.getEstimatedMyz());
20079 assertNull(calibrator.getEstimatedMzx());
20080 assertNull(calibrator.getEstimatedMzy());
20081 assertNull(calibrator.getEstimatedCovariance());
20082 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20083 }
20084
20085 @Test
20086 public void testConstructor167() throws WrongSizeException {
20087 final Collection<StandardDeviationBodyKinematics> measurements =
20088 Collections.emptyList();
20089
20090 final Matrix ba = generateBa();
20091 final double biasX = ba.getElementAtIndex(0);
20092 final double biasY = ba.getElementAtIndex(1);
20093 final double biasZ = ba.getElementAtIndex(2);
20094
20095 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20096 final double latitude = Math.toRadians(
20097 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20098 final double longitude = Math.toRadians(
20099 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20100 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20101 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20102 final NEDVelocity nedVelocity = new NEDVelocity();
20103 final ECEFPosition ecefPosition = new ECEFPosition();
20104 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20105 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20106 ecefPosition, ecefVelocity);
20107
20108 final KnownPositionAccelerometerCalibrator calibrator =
20109 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
20110 biasX, biasY, biasZ);
20111
20112
20113 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
20114 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
20115 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
20116 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20117 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20118 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20119 final Acceleration bx2 = new Acceleration(0.0,
20120 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20121 calibrator.getInitialBiasXAsAcceleration(bx2);
20122 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20123 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20124 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20125 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20126 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20127 final Acceleration by2 = new Acceleration(0.0,
20128 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20129 calibrator.getInitialBiasYAsAcceleration(by2);
20130 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20131 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20132 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20133 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20134 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20135 final Acceleration bz2 = new Acceleration(0.0,
20136 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20137 calibrator.getInitialBiasZAsAcceleration(bz2);
20138 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20139 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20140 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20141 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20142 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20143 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20144 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20145 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20146 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20147 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20148 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20149 final double[] bias1 = calibrator.getInitialBias();
20150 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
20151 final double[] bias2 = new double[3];
20152 calibrator.getInitialBias(bias2);
20153 assertArrayEquals(bias1, bias2, 0.0);
20154 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20155 assertEquals(b1, ba);
20156 final Matrix b2 = new Matrix(3, 1);
20157 calibrator.getInitialBiasAsMatrix(b2);
20158 assertEquals(b1, b2);
20159 final Matrix ma1 = calibrator.getInitialMa();
20160 assertEquals(ma1, new Matrix(3, 3));
20161 final Matrix ma2 = new Matrix(3, 3);
20162 calibrator.getInitialMa(ma2);
20163 assertEquals(ma1, ma2);
20164 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20165 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20166 final NEDPosition nedPosition1 = new NEDPosition();
20167 assertTrue(calibrator.getNedPosition(nedPosition1));
20168 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20169 assertSame(calibrator.getMeasurements(), measurements);
20170 assertFalse(calibrator.isCommonAxisUsed());
20171 assertNull(calibrator.getListener());
20172 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20173 assertFalse(calibrator.isReady());
20174 assertFalse(calibrator.isRunning());
20175 assertNull(calibrator.getEstimatedBiases());
20176 assertFalse(calibrator.getEstimatedBiases(null));
20177 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20178 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20179 assertNull(calibrator.getEstimatedBiasFx());
20180 assertNull(calibrator.getEstimatedBiasFy());
20181 assertNull(calibrator.getEstimatedBiasFz());
20182 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20183 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20184 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20185 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20186 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20187 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20188 assertNull(calibrator.getEstimatedMa());
20189 assertNull(calibrator.getEstimatedSx());
20190 assertNull(calibrator.getEstimatedSy());
20191 assertNull(calibrator.getEstimatedSz());
20192 assertNull(calibrator.getEstimatedMxy());
20193 assertNull(calibrator.getEstimatedMxz());
20194 assertNull(calibrator.getEstimatedMyx());
20195 assertNull(calibrator.getEstimatedMyz());
20196 assertNull(calibrator.getEstimatedMzx());
20197 assertNull(calibrator.getEstimatedMzy());
20198 assertNull(calibrator.getEstimatedCovariance());
20199 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20200 }
20201
20202 @Test
20203 public void testConstructor168() throws WrongSizeException {
20204 final Collection<StandardDeviationBodyKinematics> measurements =
20205 Collections.emptyList();
20206
20207 final Matrix ba = generateBa();
20208 final double biasX = ba.getElementAtIndex(0);
20209 final double biasY = ba.getElementAtIndex(1);
20210 final double biasZ = ba.getElementAtIndex(2);
20211
20212 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20213 final double latitude = Math.toRadians(
20214 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20215 final double longitude = Math.toRadians(
20216 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20217 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20218 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20219 final NEDVelocity nedVelocity = new NEDVelocity();
20220 final ECEFPosition ecefPosition = new ECEFPosition();
20221 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20222 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20223 ecefPosition, ecefVelocity);
20224
20225 final KnownPositionAccelerometerCalibrator calibrator =
20226 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
20227 biasX, biasY, biasZ, this);
20228
20229
20230 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
20231 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
20232 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
20233 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20234 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20235 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20236 final Acceleration bx2 = new Acceleration(0.0,
20237 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20238 calibrator.getInitialBiasXAsAcceleration(bx2);
20239 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20240 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20241 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20242 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20243 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20244 final Acceleration by2 = new Acceleration(0.0,
20245 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20246 calibrator.getInitialBiasYAsAcceleration(by2);
20247 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20248 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20249 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20250 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20251 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20252 final Acceleration bz2 = new Acceleration(0.0,
20253 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20254 calibrator.getInitialBiasZAsAcceleration(bz2);
20255 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20256 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20257 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20258 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20259 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20260 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20261 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20262 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20263 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20264 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20265 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20266 final double[] bias1 = calibrator.getInitialBias();
20267 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
20268 final double[] bias2 = new double[3];
20269 calibrator.getInitialBias(bias2);
20270 assertArrayEquals(bias1, bias2, 0.0);
20271 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20272 assertEquals(b1, ba);
20273 final Matrix b2 = new Matrix(3, 1);
20274 calibrator.getInitialBiasAsMatrix(b2);
20275 assertEquals(b1, b2);
20276 final Matrix ma1 = calibrator.getInitialMa();
20277 assertEquals(ma1, new Matrix(3, 3));
20278 final Matrix ma2 = new Matrix(3, 3);
20279 calibrator.getInitialMa(ma2);
20280 assertEquals(ma1, ma2);
20281 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20282 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20283 final NEDPosition nedPosition1 = new NEDPosition();
20284 assertTrue(calibrator.getNedPosition(nedPosition1));
20285 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20286 assertSame(calibrator.getMeasurements(), measurements);
20287 assertFalse(calibrator.isCommonAxisUsed());
20288 assertSame(calibrator.getListener(), this);
20289 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20290 assertFalse(calibrator.isReady());
20291 assertFalse(calibrator.isRunning());
20292 assertNull(calibrator.getEstimatedBiases());
20293 assertFalse(calibrator.getEstimatedBiases(null));
20294 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20295 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20296 assertNull(calibrator.getEstimatedBiasFx());
20297 assertNull(calibrator.getEstimatedBiasFy());
20298 assertNull(calibrator.getEstimatedBiasFz());
20299 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20300 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20301 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20302 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20303 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20304 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20305 assertNull(calibrator.getEstimatedMa());
20306 assertNull(calibrator.getEstimatedSx());
20307 assertNull(calibrator.getEstimatedSy());
20308 assertNull(calibrator.getEstimatedSz());
20309 assertNull(calibrator.getEstimatedMxy());
20310 assertNull(calibrator.getEstimatedMxz());
20311 assertNull(calibrator.getEstimatedMyx());
20312 assertNull(calibrator.getEstimatedMyz());
20313 assertNull(calibrator.getEstimatedMzx());
20314 assertNull(calibrator.getEstimatedMzy());
20315 assertNull(calibrator.getEstimatedCovariance());
20316 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20317 }
20318
20319 @Test
20320 public void testConstructor169() throws WrongSizeException {
20321 final Matrix ba = generateBa();
20322 final double biasX = ba.getElementAtIndex(0);
20323 final double biasY = ba.getElementAtIndex(1);
20324 final double biasZ = ba.getElementAtIndex(2);
20325
20326 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20327 final double latitude = Math.toRadians(
20328 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20329 final double longitude = Math.toRadians(
20330 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20331 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20332 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20333 final NEDVelocity nedVelocity = new NEDVelocity();
20334 final ECEFPosition ecefPosition = new ECEFPosition();
20335 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20336 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20337 ecefPosition, ecefVelocity);
20338
20339 final KnownPositionAccelerometerCalibrator calibrator =
20340 new KnownPositionAccelerometerCalibrator(nedPosition,
20341 true, biasX, biasY, biasZ);
20342
20343
20344 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
20345 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
20346 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
20347 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20348 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20349 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20350 final Acceleration bx2 = new Acceleration(0.0,
20351 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20352 calibrator.getInitialBiasXAsAcceleration(bx2);
20353 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20354 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20355 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20356 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20357 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20358 final Acceleration by2 = new Acceleration(0.0,
20359 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20360 calibrator.getInitialBiasYAsAcceleration(by2);
20361 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20362 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20363 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20364 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20365 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20366 final Acceleration bz2 = new Acceleration(0.0,
20367 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20368 calibrator.getInitialBiasZAsAcceleration(bz2);
20369 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20370 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20371 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20372 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20373 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20374 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20375 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20376 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20377 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20378 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20379 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20380 final double[] bias1 = calibrator.getInitialBias();
20381 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
20382 final double[] bias2 = new double[3];
20383 calibrator.getInitialBias(bias2);
20384 assertArrayEquals(bias1, bias2, 0.0);
20385 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20386 assertEquals(b1, ba);
20387 final Matrix b2 = new Matrix(3, 1);
20388 calibrator.getInitialBiasAsMatrix(b2);
20389 assertEquals(b1, b2);
20390 final Matrix ma1 = calibrator.getInitialMa();
20391 assertEquals(ma1, new Matrix(3, 3));
20392 final Matrix ma2 = new Matrix(3, 3);
20393 calibrator.getInitialMa(ma2);
20394 assertEquals(ma1, ma2);
20395 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20396 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20397 final NEDPosition nedPosition1 = new NEDPosition();
20398 assertTrue(calibrator.getNedPosition(nedPosition1));
20399 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20400 assertNull(calibrator.getMeasurements());
20401 assertTrue(calibrator.isCommonAxisUsed());
20402 assertNull(calibrator.getListener());
20403 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20404 assertFalse(calibrator.isReady());
20405 assertFalse(calibrator.isRunning());
20406 assertNull(calibrator.getEstimatedBiases());
20407 assertFalse(calibrator.getEstimatedBiases(null));
20408 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20409 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20410 assertNull(calibrator.getEstimatedBiasFx());
20411 assertNull(calibrator.getEstimatedBiasFy());
20412 assertNull(calibrator.getEstimatedBiasFz());
20413 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20414 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20415 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20416 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20417 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20418 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20419 assertNull(calibrator.getEstimatedMa());
20420 assertNull(calibrator.getEstimatedSx());
20421 assertNull(calibrator.getEstimatedSy());
20422 assertNull(calibrator.getEstimatedSz());
20423 assertNull(calibrator.getEstimatedMxy());
20424 assertNull(calibrator.getEstimatedMxz());
20425 assertNull(calibrator.getEstimatedMyx());
20426 assertNull(calibrator.getEstimatedMyz());
20427 assertNull(calibrator.getEstimatedMzx());
20428 assertNull(calibrator.getEstimatedMzy());
20429 assertNull(calibrator.getEstimatedCovariance());
20430 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20431 }
20432
20433 @Test
20434 public void testConstructor170() throws WrongSizeException {
20435 final Matrix ba = generateBa();
20436 final double biasX = ba.getElementAtIndex(0);
20437 final double biasY = ba.getElementAtIndex(1);
20438 final double biasZ = ba.getElementAtIndex(2);
20439
20440 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20441 final double latitude = Math.toRadians(
20442 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20443 final double longitude = Math.toRadians(
20444 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20445 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20446 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20447 final NEDVelocity nedVelocity = new NEDVelocity();
20448 final ECEFPosition ecefPosition = new ECEFPosition();
20449 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20450 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20451 ecefPosition, ecefVelocity);
20452
20453 final KnownPositionAccelerometerCalibrator calibrator =
20454 new KnownPositionAccelerometerCalibrator(nedPosition,
20455 true, biasX, biasY, biasZ, this);
20456
20457
20458 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
20459 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
20460 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
20461 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20462 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20463 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20464 final Acceleration bx2 = new Acceleration(0.0,
20465 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20466 calibrator.getInitialBiasXAsAcceleration(bx2);
20467 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20468 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20469 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20470 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20471 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20472 final Acceleration by2 = new Acceleration(0.0,
20473 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20474 calibrator.getInitialBiasYAsAcceleration(by2);
20475 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20476 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20477 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20478 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20479 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20480 final Acceleration bz2 = new Acceleration(0.0,
20481 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20482 calibrator.getInitialBiasZAsAcceleration(bz2);
20483 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20484 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20485 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20486 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20487 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20488 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20489 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20490 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20491 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20492 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20493 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20494 final double[] bias1 = calibrator.getInitialBias();
20495 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
20496 final double[] bias2 = new double[3];
20497 calibrator.getInitialBias(bias2);
20498 assertArrayEquals(bias1, bias2, 0.0);
20499 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20500 assertEquals(b1, ba);
20501 final Matrix b2 = new Matrix(3, 1);
20502 calibrator.getInitialBiasAsMatrix(b2);
20503 assertEquals(b1, b2);
20504 final Matrix ma1 = calibrator.getInitialMa();
20505 assertEquals(ma1, new Matrix(3, 3));
20506 final Matrix ma2 = new Matrix(3, 3);
20507 calibrator.getInitialMa(ma2);
20508 assertEquals(ma1, ma2);
20509 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20510 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20511 final NEDPosition nedPosition1 = new NEDPosition();
20512 assertTrue(calibrator.getNedPosition(nedPosition1));
20513 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20514 assertNull(calibrator.getMeasurements());
20515 assertTrue(calibrator.isCommonAxisUsed());
20516 assertSame(calibrator.getListener(), this);
20517 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20518 assertFalse(calibrator.isReady());
20519 assertFalse(calibrator.isRunning());
20520 assertNull(calibrator.getEstimatedBiases());
20521 assertFalse(calibrator.getEstimatedBiases(null));
20522 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20523 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20524 assertNull(calibrator.getEstimatedBiasFx());
20525 assertNull(calibrator.getEstimatedBiasFy());
20526 assertNull(calibrator.getEstimatedBiasFz());
20527 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20528 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20529 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20530 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20531 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20532 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20533 assertNull(calibrator.getEstimatedMa());
20534 assertNull(calibrator.getEstimatedSx());
20535 assertNull(calibrator.getEstimatedSy());
20536 assertNull(calibrator.getEstimatedSz());
20537 assertNull(calibrator.getEstimatedMxy());
20538 assertNull(calibrator.getEstimatedMxz());
20539 assertNull(calibrator.getEstimatedMyx());
20540 assertNull(calibrator.getEstimatedMyz());
20541 assertNull(calibrator.getEstimatedMzx());
20542 assertNull(calibrator.getEstimatedMzy());
20543 assertNull(calibrator.getEstimatedCovariance());
20544 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20545 }
20546
20547 @Test
20548 public void testConstructor171() throws WrongSizeException {
20549 final Collection<StandardDeviationBodyKinematics> measurements =
20550 Collections.emptyList();
20551
20552 final Matrix ba = generateBa();
20553 final double biasX = ba.getElementAtIndex(0);
20554 final double biasY = ba.getElementAtIndex(1);
20555 final double biasZ = ba.getElementAtIndex(2);
20556
20557 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20558 final double latitude = Math.toRadians(
20559 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20560 final double longitude = Math.toRadians(
20561 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20562 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20563 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20564 final NEDVelocity nedVelocity = new NEDVelocity();
20565 final ECEFPosition ecefPosition = new ECEFPosition();
20566 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20567 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20568 ecefPosition, ecefVelocity);
20569
20570 final KnownPositionAccelerometerCalibrator calibrator =
20571 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
20572 true, biasX, biasY, biasZ);
20573
20574
20575 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
20576 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
20577 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
20578 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20579 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20580 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20581 final Acceleration bx2 = new Acceleration(0.0,
20582 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20583 calibrator.getInitialBiasXAsAcceleration(bx2);
20584 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20585 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20586 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20587 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20588 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20589 final Acceleration by2 = new Acceleration(0.0,
20590 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20591 calibrator.getInitialBiasYAsAcceleration(by2);
20592 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20593 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20594 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20595 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20596 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20597 final Acceleration bz2 = new Acceleration(0.0,
20598 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20599 calibrator.getInitialBiasZAsAcceleration(bz2);
20600 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20601 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20602 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20603 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20604 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20605 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20606 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20607 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20608 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20609 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20610 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20611 final double[] bias1 = calibrator.getInitialBias();
20612 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
20613 final double[] bias2 = new double[3];
20614 calibrator.getInitialBias(bias2);
20615 assertArrayEquals(bias1, bias2, 0.0);
20616 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20617 assertEquals(b1, ba);
20618 final Matrix b2 = new Matrix(3, 1);
20619 calibrator.getInitialBiasAsMatrix(b2);
20620 assertEquals(b1, b2);
20621 final Matrix ma1 = calibrator.getInitialMa();
20622 assertEquals(ma1, new Matrix(3, 3));
20623 final Matrix ma2 = new Matrix(3, 3);
20624 calibrator.getInitialMa(ma2);
20625 assertEquals(ma1, ma2);
20626 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20627 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20628 final NEDPosition nedPosition1 = new NEDPosition();
20629 assertTrue(calibrator.getNedPosition(nedPosition1));
20630 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20631 assertSame(calibrator.getMeasurements(), measurements);
20632 assertTrue(calibrator.isCommonAxisUsed());
20633 assertNull(calibrator.getListener());
20634 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20635 assertFalse(calibrator.isReady());
20636 assertFalse(calibrator.isRunning());
20637 assertNull(calibrator.getEstimatedBiases());
20638 assertFalse(calibrator.getEstimatedBiases(null));
20639 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20640 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20641 assertNull(calibrator.getEstimatedBiasFx());
20642 assertNull(calibrator.getEstimatedBiasFy());
20643 assertNull(calibrator.getEstimatedBiasFz());
20644 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20645 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20646 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20647 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20648 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20649 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20650 assertNull(calibrator.getEstimatedMa());
20651 assertNull(calibrator.getEstimatedSx());
20652 assertNull(calibrator.getEstimatedSy());
20653 assertNull(calibrator.getEstimatedSz());
20654 assertNull(calibrator.getEstimatedMxy());
20655 assertNull(calibrator.getEstimatedMxz());
20656 assertNull(calibrator.getEstimatedMyx());
20657 assertNull(calibrator.getEstimatedMyz());
20658 assertNull(calibrator.getEstimatedMzx());
20659 assertNull(calibrator.getEstimatedMzy());
20660 assertNull(calibrator.getEstimatedCovariance());
20661 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20662 }
20663
20664 @Test
20665 public void testConstructor172() throws WrongSizeException {
20666 final Collection<StandardDeviationBodyKinematics> measurements =
20667 Collections.emptyList();
20668
20669 final Matrix ba = generateBa();
20670 final double biasX = ba.getElementAtIndex(0);
20671 final double biasY = ba.getElementAtIndex(1);
20672 final double biasZ = ba.getElementAtIndex(2);
20673
20674 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20675 final double latitude = Math.toRadians(
20676 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20677 final double longitude = Math.toRadians(
20678 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20679 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20680 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20681 final NEDVelocity nedVelocity = new NEDVelocity();
20682 final ECEFPosition ecefPosition = new ECEFPosition();
20683 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20684 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20685 ecefPosition, ecefVelocity);
20686
20687 final KnownPositionAccelerometerCalibrator calibrator =
20688 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
20689 true, biasX, biasY, biasZ, this);
20690
20691
20692 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
20693 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
20694 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
20695 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20696 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20697 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20698 final Acceleration bx2 = new Acceleration(0.0,
20699 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20700 calibrator.getInitialBiasXAsAcceleration(bx2);
20701 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20702 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20703 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20704 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20705 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20706 final Acceleration by2 = new Acceleration(0.0,
20707 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20708 calibrator.getInitialBiasYAsAcceleration(by2);
20709 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20710 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20711 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20712 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20713 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20714 final Acceleration bz2 = new Acceleration(0.0,
20715 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20716 calibrator.getInitialBiasZAsAcceleration(bz2);
20717 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20718 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20719 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20720 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20721 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20722 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20723 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20724 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20725 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20726 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20727 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20728 final double[] bias1 = calibrator.getInitialBias();
20729 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
20730 final double[] bias2 = new double[3];
20731 calibrator.getInitialBias(bias2);
20732 assertArrayEquals(bias1, bias2, 0.0);
20733 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20734 assertEquals(b1, ba);
20735 final Matrix b2 = new Matrix(3, 1);
20736 calibrator.getInitialBiasAsMatrix(b2);
20737 assertEquals(b1, b2);
20738 final Matrix ma1 = calibrator.getInitialMa();
20739 assertEquals(ma1, new Matrix(3, 3));
20740 final Matrix ma2 = new Matrix(3, 3);
20741 calibrator.getInitialMa(ma2);
20742 assertEquals(ma1, ma2);
20743 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20744 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20745 final NEDPosition nedPosition1 = new NEDPosition();
20746 assertTrue(calibrator.getNedPosition(nedPosition1));
20747 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20748 assertSame(calibrator.getMeasurements(), measurements);
20749 assertTrue(calibrator.isCommonAxisUsed());
20750 assertSame(calibrator.getListener(), this);
20751 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20752 assertFalse(calibrator.isReady());
20753 assertFalse(calibrator.isRunning());
20754 assertNull(calibrator.getEstimatedBiases());
20755 assertFalse(calibrator.getEstimatedBiases(null));
20756 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20757 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20758 assertNull(calibrator.getEstimatedBiasFx());
20759 assertNull(calibrator.getEstimatedBiasFy());
20760 assertNull(calibrator.getEstimatedBiasFz());
20761 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20762 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20763 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20764 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20765 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20766 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20767 assertNull(calibrator.getEstimatedMa());
20768 assertNull(calibrator.getEstimatedSx());
20769 assertNull(calibrator.getEstimatedSy());
20770 assertNull(calibrator.getEstimatedSz());
20771 assertNull(calibrator.getEstimatedMxy());
20772 assertNull(calibrator.getEstimatedMxz());
20773 assertNull(calibrator.getEstimatedMyx());
20774 assertNull(calibrator.getEstimatedMyz());
20775 assertNull(calibrator.getEstimatedMzx());
20776 assertNull(calibrator.getEstimatedMzy());
20777 assertNull(calibrator.getEstimatedCovariance());
20778 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20779 }
20780
20781 @Test
20782 public void testConstructor173() throws WrongSizeException {
20783 final Matrix ba = generateBa();
20784 final double biasX = ba.getElementAtIndex(0);
20785 final double biasY = ba.getElementAtIndex(1);
20786 final double biasZ = ba.getElementAtIndex(2);
20787
20788 final Acceleration bx = new Acceleration(biasX,
20789 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20790 final Acceleration by = new Acceleration(biasY,
20791 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20792 final Acceleration bz = new Acceleration(biasZ,
20793 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20794
20795 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20796 final double latitude = Math.toRadians(
20797 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20798 final double longitude = Math.toRadians(
20799 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20800 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20801 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20802 final NEDVelocity nedVelocity = new NEDVelocity();
20803 final ECEFPosition ecefPosition = new ECEFPosition();
20804 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20805 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20806 ecefPosition, ecefVelocity);
20807
20808 final KnownPositionAccelerometerCalibrator calibrator =
20809 new KnownPositionAccelerometerCalibrator(nedPosition,
20810 bx, by, bz);
20811
20812
20813 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
20814 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
20815 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
20816 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20817 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20818 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20819 final Acceleration bx2 = new Acceleration(0.0,
20820 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20821 calibrator.getInitialBiasXAsAcceleration(bx2);
20822 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20823 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20824 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20825 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20826 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20827 final Acceleration by2 = new Acceleration(0.0,
20828 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20829 calibrator.getInitialBiasYAsAcceleration(by2);
20830 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20831 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20832 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20833 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20834 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20835 final Acceleration bz2 = new Acceleration(0.0,
20836 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20837 calibrator.getInitialBiasZAsAcceleration(bz2);
20838 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20839 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20840 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20841 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20842 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20843 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20844 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20845 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20846 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20847 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20848 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20849 final double[] bias1 = calibrator.getInitialBias();
20850 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
20851 final double[] bias2 = new double[3];
20852 calibrator.getInitialBias(bias2);
20853 assertArrayEquals(bias1, bias2, 0.0);
20854 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20855 assertEquals(b1, ba);
20856 final Matrix b2 = new Matrix(3, 1);
20857 calibrator.getInitialBiasAsMatrix(b2);
20858 assertEquals(b1, b2);
20859 final Matrix ma1 = calibrator.getInitialMa();
20860 assertEquals(ma1, new Matrix(3, 3));
20861 final Matrix ma2 = new Matrix(3, 3);
20862 calibrator.getInitialMa(ma2);
20863 assertEquals(ma1, ma2);
20864 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20865 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20866 final NEDPosition nedPosition1 = new NEDPosition();
20867 assertTrue(calibrator.getNedPosition(nedPosition1));
20868 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20869 assertNull(calibrator.getMeasurements());
20870 assertFalse(calibrator.isCommonAxisUsed());
20871 assertNull(calibrator.getListener());
20872 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20873 assertFalse(calibrator.isReady());
20874 assertFalse(calibrator.isRunning());
20875 assertNull(calibrator.getEstimatedBiases());
20876 assertFalse(calibrator.getEstimatedBiases(null));
20877 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20878 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20879 assertNull(calibrator.getEstimatedBiasFx());
20880 assertNull(calibrator.getEstimatedBiasFy());
20881 assertNull(calibrator.getEstimatedBiasFz());
20882 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20883 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20884 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20885 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20886 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20887 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20888 assertNull(calibrator.getEstimatedMa());
20889 assertNull(calibrator.getEstimatedSx());
20890 assertNull(calibrator.getEstimatedSy());
20891 assertNull(calibrator.getEstimatedSz());
20892 assertNull(calibrator.getEstimatedMxy());
20893 assertNull(calibrator.getEstimatedMxz());
20894 assertNull(calibrator.getEstimatedMyx());
20895 assertNull(calibrator.getEstimatedMyz());
20896 assertNull(calibrator.getEstimatedMzx());
20897 assertNull(calibrator.getEstimatedMzy());
20898 assertNull(calibrator.getEstimatedCovariance());
20899 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20900 }
20901
20902 @Test
20903 public void testConstructor174() throws WrongSizeException {
20904 final Matrix ba = generateBa();
20905 final double biasX = ba.getElementAtIndex(0);
20906 final double biasY = ba.getElementAtIndex(1);
20907 final double biasZ = ba.getElementAtIndex(2);
20908
20909 final Acceleration bx = new Acceleration(biasX,
20910 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20911 final Acceleration by = new Acceleration(biasY,
20912 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20913 final Acceleration bz = new Acceleration(biasZ,
20914 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20915
20916 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20917 final double latitude = Math.toRadians(
20918 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20919 final double longitude = Math.toRadians(
20920 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20921 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20922 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20923 final NEDVelocity nedVelocity = new NEDVelocity();
20924 final ECEFPosition ecefPosition = new ECEFPosition();
20925 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20926 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20927 ecefPosition, ecefVelocity);
20928
20929 final KnownPositionAccelerometerCalibrator calibrator =
20930 new KnownPositionAccelerometerCalibrator(nedPosition,
20931 bx, by, bz, this);
20932
20933
20934 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
20935 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
20936 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
20937 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20938 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20939 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20940 final Acceleration bx2 = new Acceleration(0.0,
20941 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20942 calibrator.getInitialBiasXAsAcceleration(bx2);
20943 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20944 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20945 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20946 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20947 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20948 final Acceleration by2 = new Acceleration(0.0,
20949 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20950 calibrator.getInitialBiasYAsAcceleration(by2);
20951 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20952 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20953 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20954 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20955 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20956 final Acceleration bz2 = new Acceleration(0.0,
20957 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20958 calibrator.getInitialBiasZAsAcceleration(bz2);
20959 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20960 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20961 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20962 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20963 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20964 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20965 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20966 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20967 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20968 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20969 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20970 final double[] bias1 = calibrator.getInitialBias();
20971 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
20972 final double[] bias2 = new double[3];
20973 calibrator.getInitialBias(bias2);
20974 assertArrayEquals(bias1, bias2, 0.0);
20975 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20976 assertEquals(b1, ba);
20977 final Matrix b2 = new Matrix(3, 1);
20978 calibrator.getInitialBiasAsMatrix(b2);
20979 assertEquals(b1, b2);
20980 final Matrix ma1 = calibrator.getInitialMa();
20981 assertEquals(ma1, new Matrix(3, 3));
20982 final Matrix ma2 = new Matrix(3, 3);
20983 calibrator.getInitialMa(ma2);
20984 assertEquals(ma1, ma2);
20985 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20986 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20987 final NEDPosition nedPosition1 = new NEDPosition();
20988 assertTrue(calibrator.getNedPosition(nedPosition1));
20989 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20990 assertNull(calibrator.getMeasurements());
20991 assertFalse(calibrator.isCommonAxisUsed());
20992 assertSame(calibrator.getListener(), this);
20993 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20994 assertFalse(calibrator.isReady());
20995 assertFalse(calibrator.isRunning());
20996 assertNull(calibrator.getEstimatedBiases());
20997 assertFalse(calibrator.getEstimatedBiases(null));
20998 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20999 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21000 assertNull(calibrator.getEstimatedBiasFx());
21001 assertNull(calibrator.getEstimatedBiasFy());
21002 assertNull(calibrator.getEstimatedBiasFz());
21003 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21004 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21005 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21006 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21007 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21008 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21009 assertNull(calibrator.getEstimatedMa());
21010 assertNull(calibrator.getEstimatedSx());
21011 assertNull(calibrator.getEstimatedSy());
21012 assertNull(calibrator.getEstimatedSz());
21013 assertNull(calibrator.getEstimatedMxy());
21014 assertNull(calibrator.getEstimatedMxz());
21015 assertNull(calibrator.getEstimatedMyx());
21016 assertNull(calibrator.getEstimatedMyz());
21017 assertNull(calibrator.getEstimatedMzx());
21018 assertNull(calibrator.getEstimatedMzy());
21019 assertNull(calibrator.getEstimatedCovariance());
21020 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21021 }
21022
21023 @Test
21024 public void testConstructor175() throws WrongSizeException {
21025 final Collection<StandardDeviationBodyKinematics> measurements =
21026 Collections.emptyList();
21027
21028 final Matrix ba = generateBa();
21029 final double biasX = ba.getElementAtIndex(0);
21030 final double biasY = ba.getElementAtIndex(1);
21031 final double biasZ = ba.getElementAtIndex(2);
21032
21033 final Acceleration bx = new Acceleration(biasX,
21034 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21035 final Acceleration by = new Acceleration(biasY,
21036 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21037 final Acceleration bz = new Acceleration(biasZ,
21038 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21039
21040 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21041 final double latitude = Math.toRadians(
21042 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21043 final double longitude = Math.toRadians(
21044 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21045 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21046 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21047 final NEDVelocity nedVelocity = new NEDVelocity();
21048 final ECEFPosition ecefPosition = new ECEFPosition();
21049 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21050 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21051 ecefPosition, ecefVelocity);
21052
21053 final KnownPositionAccelerometerCalibrator calibrator =
21054 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
21055 bx, by, bz);
21056
21057
21058 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21059 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21060 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21061 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21062 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21063 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21064 final Acceleration bx2 = new Acceleration(0.0,
21065 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21066 calibrator.getInitialBiasXAsAcceleration(bx2);
21067 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21068 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21069 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21070 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21071 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21072 final Acceleration by2 = new Acceleration(0.0,
21073 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21074 calibrator.getInitialBiasYAsAcceleration(by2);
21075 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21076 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21077 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21078 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21079 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21080 final Acceleration bz2 = new Acceleration(0.0,
21081 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21082 calibrator.getInitialBiasZAsAcceleration(bz2);
21083 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21084 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21085 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21086 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21087 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21088 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21089 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21090 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21091 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21092 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21093 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21094 final double[] bias1 = calibrator.getInitialBias();
21095 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
21096 final double[] bias2 = new double[3];
21097 calibrator.getInitialBias(bias2);
21098 assertArrayEquals(bias1, bias2, 0.0);
21099 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21100 assertEquals(b1, ba);
21101 final Matrix b2 = new Matrix(3, 1);
21102 calibrator.getInitialBiasAsMatrix(b2);
21103 assertEquals(b1, b2);
21104 final Matrix ma1 = calibrator.getInitialMa();
21105 assertEquals(ma1, new Matrix(3, 3));
21106 final Matrix ma2 = new Matrix(3, 3);
21107 calibrator.getInitialMa(ma2);
21108 assertEquals(ma1, ma2);
21109 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21110 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21111 final NEDPosition nedPosition1 = new NEDPosition();
21112 assertTrue(calibrator.getNedPosition(nedPosition1));
21113 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21114 assertSame(calibrator.getMeasurements(), measurements);
21115 assertFalse(calibrator.isCommonAxisUsed());
21116 assertNull(calibrator.getListener());
21117 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21118 assertFalse(calibrator.isReady());
21119 assertFalse(calibrator.isRunning());
21120 assertNull(calibrator.getEstimatedBiases());
21121 assertFalse(calibrator.getEstimatedBiases(null));
21122 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21123 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21124 assertNull(calibrator.getEstimatedBiasFx());
21125 assertNull(calibrator.getEstimatedBiasFy());
21126 assertNull(calibrator.getEstimatedBiasFz());
21127 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21128 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21129 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21130 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21131 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21132 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21133 assertNull(calibrator.getEstimatedMa());
21134 assertNull(calibrator.getEstimatedSx());
21135 assertNull(calibrator.getEstimatedSy());
21136 assertNull(calibrator.getEstimatedSz());
21137 assertNull(calibrator.getEstimatedMxy());
21138 assertNull(calibrator.getEstimatedMxz());
21139 assertNull(calibrator.getEstimatedMyx());
21140 assertNull(calibrator.getEstimatedMyz());
21141 assertNull(calibrator.getEstimatedMzx());
21142 assertNull(calibrator.getEstimatedMzy());
21143 assertNull(calibrator.getEstimatedCovariance());
21144 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21145 }
21146
21147 @Test
21148 public void testConstructor176() throws WrongSizeException {
21149 final Collection<StandardDeviationBodyKinematics> measurements =
21150 Collections.emptyList();
21151
21152 final Matrix ba = generateBa();
21153 final double biasX = ba.getElementAtIndex(0);
21154 final double biasY = ba.getElementAtIndex(1);
21155 final double biasZ = ba.getElementAtIndex(2);
21156
21157 final Acceleration bx = new Acceleration(biasX,
21158 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21159 final Acceleration by = new Acceleration(biasY,
21160 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21161 final Acceleration bz = new Acceleration(biasZ,
21162 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21163
21164 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21165 final double latitude = Math.toRadians(
21166 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21167 final double longitude = Math.toRadians(
21168 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21169 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21170 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21171 final NEDVelocity nedVelocity = new NEDVelocity();
21172 final ECEFPosition ecefPosition = new ECEFPosition();
21173 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21174 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21175 ecefPosition, ecefVelocity);
21176
21177 final KnownPositionAccelerometerCalibrator calibrator =
21178 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
21179 bx, by, bz, this);
21180
21181
21182 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21183 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21184 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21185 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21186 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21187 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21188 final Acceleration bx2 = new Acceleration(0.0,
21189 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21190 calibrator.getInitialBiasXAsAcceleration(bx2);
21191 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21192 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21193 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21194 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21195 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21196 final Acceleration by2 = new Acceleration(0.0,
21197 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21198 calibrator.getInitialBiasYAsAcceleration(by2);
21199 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21200 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21201 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21202 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21203 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21204 final Acceleration bz2 = new Acceleration(0.0,
21205 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21206 calibrator.getInitialBiasZAsAcceleration(bz2);
21207 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21208 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21209 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21210 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21211 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21212 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21213 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21214 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21215 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21216 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21217 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21218 final double[] bias1 = calibrator.getInitialBias();
21219 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
21220 final double[] bias2 = new double[3];
21221 calibrator.getInitialBias(bias2);
21222 assertArrayEquals(bias1, bias2, 0.0);
21223 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21224 assertEquals(b1, ba);
21225 final Matrix b2 = new Matrix(3, 1);
21226 calibrator.getInitialBiasAsMatrix(b2);
21227 assertEquals(b1, b2);
21228 final Matrix ma1 = calibrator.getInitialMa();
21229 assertEquals(ma1, new Matrix(3, 3));
21230 final Matrix ma2 = new Matrix(3, 3);
21231 calibrator.getInitialMa(ma2);
21232 assertEquals(ma1, ma2);
21233 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21234 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21235 final NEDPosition nedPosition1 = new NEDPosition();
21236 assertTrue(calibrator.getNedPosition(nedPosition1));
21237 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21238 assertSame(calibrator.getMeasurements(), measurements);
21239 assertFalse(calibrator.isCommonAxisUsed());
21240 assertSame(calibrator.getListener(), this);
21241 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21242 assertFalse(calibrator.isReady());
21243 assertFalse(calibrator.isRunning());
21244 assertNull(calibrator.getEstimatedBiases());
21245 assertFalse(calibrator.getEstimatedBiases(null));
21246 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21247 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21248 assertNull(calibrator.getEstimatedBiasFx());
21249 assertNull(calibrator.getEstimatedBiasFy());
21250 assertNull(calibrator.getEstimatedBiasFz());
21251 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21252 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21253 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21254 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21255 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21256 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21257 assertNull(calibrator.getEstimatedMa());
21258 assertNull(calibrator.getEstimatedSx());
21259 assertNull(calibrator.getEstimatedSy());
21260 assertNull(calibrator.getEstimatedSz());
21261 assertNull(calibrator.getEstimatedMxy());
21262 assertNull(calibrator.getEstimatedMxz());
21263 assertNull(calibrator.getEstimatedMyx());
21264 assertNull(calibrator.getEstimatedMyz());
21265 assertNull(calibrator.getEstimatedMzx());
21266 assertNull(calibrator.getEstimatedMzy());
21267 assertNull(calibrator.getEstimatedCovariance());
21268 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21269 }
21270
21271 @Test
21272 public void testConstructor177() throws WrongSizeException {
21273 final Matrix ba = generateBa();
21274 final double biasX = ba.getElementAtIndex(0);
21275 final double biasY = ba.getElementAtIndex(1);
21276 final double biasZ = ba.getElementAtIndex(2);
21277
21278 final Acceleration bx = new Acceleration(biasX,
21279 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21280 final Acceleration by = new Acceleration(biasY,
21281 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21282 final Acceleration bz = new Acceleration(biasZ,
21283 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21284
21285 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21286 final double latitude = Math.toRadians(
21287 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21288 final double longitude = Math.toRadians(
21289 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21290 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21291 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21292 final NEDVelocity nedVelocity = new NEDVelocity();
21293 final ECEFPosition ecefPosition = new ECEFPosition();
21294 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21295 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21296 ecefPosition, ecefVelocity);
21297
21298 final KnownPositionAccelerometerCalibrator calibrator =
21299 new KnownPositionAccelerometerCalibrator(nedPosition,
21300 true, bx, by, bz);
21301
21302
21303 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21304 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21305 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21306 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21307 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21308 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21309 final Acceleration bx2 = new Acceleration(0.0,
21310 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21311 calibrator.getInitialBiasXAsAcceleration(bx2);
21312 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21313 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21314 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21315 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21316 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21317 final Acceleration by2 = new Acceleration(0.0,
21318 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21319 calibrator.getInitialBiasYAsAcceleration(by2);
21320 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21321 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21322 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21323 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21324 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21325 final Acceleration bz2 = new Acceleration(0.0,
21326 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21327 calibrator.getInitialBiasZAsAcceleration(bz2);
21328 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21329 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21330 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21331 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21332 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21333 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21334 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21335 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21336 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21337 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21338 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21339 final double[] bias1 = calibrator.getInitialBias();
21340 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
21341 final double[] bias2 = new double[3];
21342 calibrator.getInitialBias(bias2);
21343 assertArrayEquals(bias1, bias2, 0.0);
21344 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21345 assertEquals(b1, ba);
21346 final Matrix b2 = new Matrix(3, 1);
21347 calibrator.getInitialBiasAsMatrix(b2);
21348 assertEquals(b1, b2);
21349 final Matrix ma1 = calibrator.getInitialMa();
21350 assertEquals(ma1, new Matrix(3, 3));
21351 final Matrix ma2 = new Matrix(3, 3);
21352 calibrator.getInitialMa(ma2);
21353 assertEquals(ma1, ma2);
21354 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21355 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21356 final NEDPosition nedPosition1 = new NEDPosition();
21357 assertTrue(calibrator.getNedPosition(nedPosition1));
21358 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21359 assertNull(calibrator.getMeasurements());
21360 assertTrue(calibrator.isCommonAxisUsed());
21361 assertNull(calibrator.getListener());
21362 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21363 assertFalse(calibrator.isReady());
21364 assertFalse(calibrator.isRunning());
21365 assertNull(calibrator.getEstimatedBiases());
21366 assertFalse(calibrator.getEstimatedBiases(null));
21367 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21368 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21369 assertNull(calibrator.getEstimatedBiasFx());
21370 assertNull(calibrator.getEstimatedBiasFy());
21371 assertNull(calibrator.getEstimatedBiasFz());
21372 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21373 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21374 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21375 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21376 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21377 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21378 assertNull(calibrator.getEstimatedMa());
21379 assertNull(calibrator.getEstimatedSx());
21380 assertNull(calibrator.getEstimatedSy());
21381 assertNull(calibrator.getEstimatedSz());
21382 assertNull(calibrator.getEstimatedMxy());
21383 assertNull(calibrator.getEstimatedMxz());
21384 assertNull(calibrator.getEstimatedMyx());
21385 assertNull(calibrator.getEstimatedMyz());
21386 assertNull(calibrator.getEstimatedMzx());
21387 assertNull(calibrator.getEstimatedMzy());
21388 assertNull(calibrator.getEstimatedCovariance());
21389 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21390 }
21391
21392 @Test
21393 public void testConstructor178() throws WrongSizeException {
21394 final Matrix ba = generateBa();
21395 final double biasX = ba.getElementAtIndex(0);
21396 final double biasY = ba.getElementAtIndex(1);
21397 final double biasZ = ba.getElementAtIndex(2);
21398
21399 final Acceleration bx = new Acceleration(biasX,
21400 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21401 final Acceleration by = new Acceleration(biasY,
21402 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21403 final Acceleration bz = new Acceleration(biasZ,
21404 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21405
21406 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21407 final double latitude = Math.toRadians(
21408 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21409 final double longitude = Math.toRadians(
21410 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21411 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21412 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21413 final NEDVelocity nedVelocity = new NEDVelocity();
21414 final ECEFPosition ecefPosition = new ECEFPosition();
21415 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21416 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21417 ecefPosition, ecefVelocity);
21418
21419 final KnownPositionAccelerometerCalibrator calibrator =
21420 new KnownPositionAccelerometerCalibrator(nedPosition,
21421 true, bx, by, bz, this);
21422
21423
21424 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21425 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21426 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21427 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21428 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21429 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21430 final Acceleration bx2 = new Acceleration(0.0,
21431 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21432 calibrator.getInitialBiasXAsAcceleration(bx2);
21433 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21434 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21435 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21436 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21437 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21438 final Acceleration by2 = new Acceleration(0.0,
21439 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21440 calibrator.getInitialBiasYAsAcceleration(by2);
21441 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21442 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21443 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21444 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21445 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21446 final Acceleration bz2 = new Acceleration(0.0,
21447 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21448 calibrator.getInitialBiasZAsAcceleration(bz2);
21449 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21450 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21451 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21452 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21453 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21454 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21455 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21456 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21457 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21458 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21459 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21460 final double[] bias1 = calibrator.getInitialBias();
21461 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
21462 final double[] bias2 = new double[3];
21463 calibrator.getInitialBias(bias2);
21464 assertArrayEquals(bias1, bias2, 0.0);
21465 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21466 assertEquals(b1, ba);
21467 final Matrix b2 = new Matrix(3, 1);
21468 calibrator.getInitialBiasAsMatrix(b2);
21469 assertEquals(b1, b2);
21470 final Matrix ma1 = calibrator.getInitialMa();
21471 assertEquals(ma1, new Matrix(3, 3));
21472 final Matrix ma2 = new Matrix(3, 3);
21473 calibrator.getInitialMa(ma2);
21474 assertEquals(ma1, ma2);
21475 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21476 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21477 final NEDPosition nedPosition1 = new NEDPosition();
21478 assertTrue(calibrator.getNedPosition(nedPosition1));
21479 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21480 assertNull(calibrator.getMeasurements());
21481 assertTrue(calibrator.isCommonAxisUsed());
21482 assertSame(calibrator.getListener(), this);
21483 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21484 assertFalse(calibrator.isReady());
21485 assertFalse(calibrator.isRunning());
21486 assertNull(calibrator.getEstimatedBiases());
21487 assertFalse(calibrator.getEstimatedBiases(null));
21488 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21489 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21490 assertNull(calibrator.getEstimatedBiasFx());
21491 assertNull(calibrator.getEstimatedBiasFy());
21492 assertNull(calibrator.getEstimatedBiasFz());
21493 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21494 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21495 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21496 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21497 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21498 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21499 assertNull(calibrator.getEstimatedMa());
21500 assertNull(calibrator.getEstimatedSx());
21501 assertNull(calibrator.getEstimatedSy());
21502 assertNull(calibrator.getEstimatedSz());
21503 assertNull(calibrator.getEstimatedMxy());
21504 assertNull(calibrator.getEstimatedMxz());
21505 assertNull(calibrator.getEstimatedMyx());
21506 assertNull(calibrator.getEstimatedMyz());
21507 assertNull(calibrator.getEstimatedMzx());
21508 assertNull(calibrator.getEstimatedMzy());
21509 assertNull(calibrator.getEstimatedCovariance());
21510 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21511 }
21512
21513 @Test
21514 public void testConstructor179() throws WrongSizeException {
21515 final Collection<StandardDeviationBodyKinematics> measurements =
21516 Collections.emptyList();
21517
21518 final Matrix ba = generateBa();
21519 final double biasX = ba.getElementAtIndex(0);
21520 final double biasY = ba.getElementAtIndex(1);
21521 final double biasZ = ba.getElementAtIndex(2);
21522
21523 final Acceleration bx = new Acceleration(biasX,
21524 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21525 final Acceleration by = new Acceleration(biasY,
21526 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21527 final Acceleration bz = new Acceleration(biasZ,
21528 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21529
21530 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21531 final double latitude = Math.toRadians(
21532 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21533 final double longitude = Math.toRadians(
21534 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21535 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21536 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21537 final NEDVelocity nedVelocity = new NEDVelocity();
21538 final ECEFPosition ecefPosition = new ECEFPosition();
21539 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21540 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21541 ecefPosition, ecefVelocity);
21542
21543 final KnownPositionAccelerometerCalibrator calibrator =
21544 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
21545 true, bx, by, bz);
21546
21547
21548 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21549 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21550 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21551 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21552 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21553 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21554 final Acceleration bx2 = new Acceleration(0.0,
21555 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21556 calibrator.getInitialBiasXAsAcceleration(bx2);
21557 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21558 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21559 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21560 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21561 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21562 final Acceleration by2 = new Acceleration(0.0,
21563 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21564 calibrator.getInitialBiasYAsAcceleration(by2);
21565 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21566 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21567 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21568 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21569 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21570 final Acceleration bz2 = new Acceleration(0.0,
21571 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21572 calibrator.getInitialBiasZAsAcceleration(bz2);
21573 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21574 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21575 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21576 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21577 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21578 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21579 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21580 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21581 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21582 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21583 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21584 final double[] bias1 = calibrator.getInitialBias();
21585 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
21586 final double[] bias2 = new double[3];
21587 calibrator.getInitialBias(bias2);
21588 assertArrayEquals(bias1, bias2, 0.0);
21589 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21590 assertEquals(b1, ba);
21591 final Matrix b2 = new Matrix(3, 1);
21592 calibrator.getInitialBiasAsMatrix(b2);
21593 assertEquals(b1, b2);
21594 final Matrix ma1 = calibrator.getInitialMa();
21595 assertEquals(ma1, new Matrix(3, 3));
21596 final Matrix ma2 = new Matrix(3, 3);
21597 calibrator.getInitialMa(ma2);
21598 assertEquals(ma1, ma2);
21599 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21600 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21601 final NEDPosition nedPosition1 = new NEDPosition();
21602 assertTrue(calibrator.getNedPosition(nedPosition1));
21603 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21604 assertSame(calibrator.getMeasurements(), measurements);
21605 assertTrue(calibrator.isCommonAxisUsed());
21606 assertNull(calibrator.getListener());
21607 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21608 assertFalse(calibrator.isReady());
21609 assertFalse(calibrator.isRunning());
21610 assertNull(calibrator.getEstimatedBiases());
21611 assertFalse(calibrator.getEstimatedBiases(null));
21612 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21613 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21614 assertNull(calibrator.getEstimatedBiasFx());
21615 assertNull(calibrator.getEstimatedBiasFy());
21616 assertNull(calibrator.getEstimatedBiasFz());
21617 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21618 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21619 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21620 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21621 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21622 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21623 assertNull(calibrator.getEstimatedMa());
21624 assertNull(calibrator.getEstimatedSx());
21625 assertNull(calibrator.getEstimatedSy());
21626 assertNull(calibrator.getEstimatedSz());
21627 assertNull(calibrator.getEstimatedMxy());
21628 assertNull(calibrator.getEstimatedMxz());
21629 assertNull(calibrator.getEstimatedMyx());
21630 assertNull(calibrator.getEstimatedMyz());
21631 assertNull(calibrator.getEstimatedMzx());
21632 assertNull(calibrator.getEstimatedMzy());
21633 assertNull(calibrator.getEstimatedCovariance());
21634 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21635 }
21636
21637 @Test
21638 public void testConstructor180() throws WrongSizeException {
21639 final Collection<StandardDeviationBodyKinematics> measurements =
21640 Collections.emptyList();
21641
21642 final Matrix ba = generateBa();
21643 final double biasX = ba.getElementAtIndex(0);
21644 final double biasY = ba.getElementAtIndex(1);
21645 final double biasZ = ba.getElementAtIndex(2);
21646
21647 final Acceleration bx = new Acceleration(biasX,
21648 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21649 final Acceleration by = new Acceleration(biasY,
21650 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21651 final Acceleration bz = new Acceleration(biasZ,
21652 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21653
21654 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21655 final double latitude = Math.toRadians(
21656 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21657 final double longitude = Math.toRadians(
21658 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21659 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21660 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21661 final NEDVelocity nedVelocity = new NEDVelocity();
21662 final ECEFPosition ecefPosition = new ECEFPosition();
21663 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21664 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21665 ecefPosition, ecefVelocity);
21666
21667 final KnownPositionAccelerometerCalibrator calibrator =
21668 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
21669 true, bx, by, bz, this);
21670
21671
21672 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21673 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21674 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21675 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21676 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21677 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21678 final Acceleration bx2 = new Acceleration(0.0,
21679 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21680 calibrator.getInitialBiasXAsAcceleration(bx2);
21681 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21682 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21683 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21684 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21685 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21686 final Acceleration by2 = new Acceleration(0.0,
21687 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21688 calibrator.getInitialBiasYAsAcceleration(by2);
21689 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21690 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21691 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21692 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21693 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21694 final Acceleration bz2 = new Acceleration(0.0,
21695 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21696 calibrator.getInitialBiasZAsAcceleration(bz2);
21697 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21698 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21699 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21700 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21701 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21702 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21703 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21704 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21705 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21706 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21707 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21708 final double[] bias1 = calibrator.getInitialBias();
21709 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
21710 final double[] bias2 = new double[3];
21711 calibrator.getInitialBias(bias2);
21712 assertArrayEquals(bias1, bias2, 0.0);
21713 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21714 assertEquals(b1, ba);
21715 final Matrix b2 = new Matrix(3, 1);
21716 calibrator.getInitialBiasAsMatrix(b2);
21717 assertEquals(b1, b2);
21718 final Matrix ma1 = calibrator.getInitialMa();
21719 assertEquals(ma1, new Matrix(3, 3));
21720 final Matrix ma2 = new Matrix(3, 3);
21721 calibrator.getInitialMa(ma2);
21722 assertEquals(ma1, ma2);
21723 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21724 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21725 final NEDPosition nedPosition1 = new NEDPosition();
21726 assertTrue(calibrator.getNedPosition(nedPosition1));
21727 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21728 assertSame(calibrator.getMeasurements(), measurements);
21729 assertTrue(calibrator.isCommonAxisUsed());
21730 assertSame(calibrator.getListener(), this);
21731 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21732 assertFalse(calibrator.isReady());
21733 assertFalse(calibrator.isRunning());
21734 assertNull(calibrator.getEstimatedBiases());
21735 assertFalse(calibrator.getEstimatedBiases(null));
21736 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21737 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21738 assertNull(calibrator.getEstimatedBiasFx());
21739 assertNull(calibrator.getEstimatedBiasFy());
21740 assertNull(calibrator.getEstimatedBiasFz());
21741 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21742 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21743 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21744 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21745 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21746 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21747 assertNull(calibrator.getEstimatedMa());
21748 assertNull(calibrator.getEstimatedSx());
21749 assertNull(calibrator.getEstimatedSy());
21750 assertNull(calibrator.getEstimatedSz());
21751 assertNull(calibrator.getEstimatedMxy());
21752 assertNull(calibrator.getEstimatedMxz());
21753 assertNull(calibrator.getEstimatedMyx());
21754 assertNull(calibrator.getEstimatedMyz());
21755 assertNull(calibrator.getEstimatedMzx());
21756 assertNull(calibrator.getEstimatedMzy());
21757 assertNull(calibrator.getEstimatedCovariance());
21758 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21759 }
21760
21761 @Test
21762 public void testConstructor181() throws WrongSizeException {
21763 final Matrix ba = generateBa();
21764 final double biasX = ba.getElementAtIndex(0);
21765 final double biasY = ba.getElementAtIndex(1);
21766 final double biasZ = ba.getElementAtIndex(2);
21767
21768 final Matrix ma = generateMaCommonAxis();
21769 final double sx = ma.getElementAt(0, 0);
21770 final double sy = ma.getElementAt(1, 1);
21771 final double sz = ma.getElementAt(2, 2);
21772
21773 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21774 final double latitude = Math.toRadians(
21775 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21776 final double longitude = Math.toRadians(
21777 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21778 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21779 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21780 final NEDVelocity nedVelocity = new NEDVelocity();
21781 final ECEFPosition ecefPosition = new ECEFPosition();
21782 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21783 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21784 ecefPosition, ecefVelocity);
21785
21786 final KnownPositionAccelerometerCalibrator calibrator =
21787 new KnownPositionAccelerometerCalibrator(nedPosition,
21788 biasX, biasY, biasZ, sx, sy, sz);
21789
21790
21791 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21792 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21793 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21794 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21795 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21796 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21797 final Acceleration bx2 = new Acceleration(0.0,
21798 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21799 calibrator.getInitialBiasXAsAcceleration(bx2);
21800 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21801 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21802 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21803 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21804 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21805 final Acceleration by2 = new Acceleration(0.0,
21806 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21807 calibrator.getInitialBiasYAsAcceleration(by2);
21808 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21809 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21810 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21811 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21812 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21813 final Acceleration bz2 = new Acceleration(0.0,
21814 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21815 calibrator.getInitialBiasZAsAcceleration(bz2);
21816 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21817 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21818 assertEquals(calibrator.getInitialSx(), sx, 0.0);
21819 assertEquals(calibrator.getInitialSy(), sy, 0.0);
21820 assertEquals(calibrator.getInitialSz(), sz, 0.0);
21821 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21822 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21823 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21824 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21825 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21826 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21827 final double[] bias1 = calibrator.getInitialBias();
21828 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
21829 final double[] bias2 = new double[3];
21830 calibrator.getInitialBias(bias2);
21831 assertArrayEquals(bias1, bias2, 0.0);
21832 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21833 assertEquals(b1, ba);
21834 final Matrix b2 = new Matrix(3, 1);
21835 calibrator.getInitialBiasAsMatrix(b2);
21836 assertEquals(b1, b2);
21837 final Matrix ma1 = calibrator.getInitialMa();
21838 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
21839 final Matrix ma2 = new Matrix(3, 3);
21840 calibrator.getInitialMa(ma2);
21841 assertEquals(ma1, ma2);
21842 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21843 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21844 final NEDPosition nedPosition1 = new NEDPosition();
21845 assertTrue(calibrator.getNedPosition(nedPosition1));
21846 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21847 assertNull(calibrator.getMeasurements());
21848 assertFalse(calibrator.isCommonAxisUsed());
21849 assertNull(calibrator.getListener());
21850 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21851 assertFalse(calibrator.isReady());
21852 assertFalse(calibrator.isRunning());
21853 assertNull(calibrator.getEstimatedBiases());
21854 assertFalse(calibrator.getEstimatedBiases(null));
21855 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21856 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21857 assertNull(calibrator.getEstimatedBiasFx());
21858 assertNull(calibrator.getEstimatedBiasFy());
21859 assertNull(calibrator.getEstimatedBiasFz());
21860 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21861 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21862 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21863 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21864 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21865 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21866 assertNull(calibrator.getEstimatedMa());
21867 assertNull(calibrator.getEstimatedSx());
21868 assertNull(calibrator.getEstimatedSy());
21869 assertNull(calibrator.getEstimatedSz());
21870 assertNull(calibrator.getEstimatedMxy());
21871 assertNull(calibrator.getEstimatedMxz());
21872 assertNull(calibrator.getEstimatedMyx());
21873 assertNull(calibrator.getEstimatedMyz());
21874 assertNull(calibrator.getEstimatedMzx());
21875 assertNull(calibrator.getEstimatedMzy());
21876 assertNull(calibrator.getEstimatedCovariance());
21877 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21878 }
21879
21880 @Test
21881 public void testConstructor182() throws WrongSizeException {
21882 final Collection<StandardDeviationBodyKinematics> measurements =
21883 Collections.emptyList();
21884
21885 final Matrix ba = generateBa();
21886 final double biasX = ba.getElementAtIndex(0);
21887 final double biasY = ba.getElementAtIndex(1);
21888 final double biasZ = ba.getElementAtIndex(2);
21889
21890 final Matrix ma = generateMaCommonAxis();
21891 final double sx = ma.getElementAt(0, 0);
21892 final double sy = ma.getElementAt(1, 1);
21893 final double sz = ma.getElementAt(2, 2);
21894
21895 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21896 final double latitude = Math.toRadians(
21897 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21898 final double longitude = Math.toRadians(
21899 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21900 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21901 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21902 final NEDVelocity nedVelocity = new NEDVelocity();
21903 final ECEFPosition ecefPosition = new ECEFPosition();
21904 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21905 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21906 ecefPosition, ecefVelocity);
21907
21908 final KnownPositionAccelerometerCalibrator calibrator =
21909 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
21910 biasX, biasY, biasZ, sx, sy, sz);
21911
21912
21913 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21914 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21915 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21916 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21917 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21918 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21919 final Acceleration bx2 = new Acceleration(0.0,
21920 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21921 calibrator.getInitialBiasXAsAcceleration(bx2);
21922 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21923 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21924 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21925 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21926 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21927 final Acceleration by2 = new Acceleration(0.0,
21928 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21929 calibrator.getInitialBiasYAsAcceleration(by2);
21930 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21931 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21932 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21933 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21934 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21935 final Acceleration bz2 = new Acceleration(0.0,
21936 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21937 calibrator.getInitialBiasZAsAcceleration(bz2);
21938 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21939 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21940 assertEquals(calibrator.getInitialSx(), sx, 0.0);
21941 assertEquals(calibrator.getInitialSy(), sy, 0.0);
21942 assertEquals(calibrator.getInitialSz(), sz, 0.0);
21943 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21944 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21945 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21946 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21947 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21948 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21949 final double[] bias1 = calibrator.getInitialBias();
21950 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
21951 final double[] bias2 = new double[3];
21952 calibrator.getInitialBias(bias2);
21953 assertArrayEquals(bias1, bias2, 0.0);
21954 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21955 assertEquals(b1, ba);
21956 final Matrix b2 = new Matrix(3, 1);
21957 calibrator.getInitialBiasAsMatrix(b2);
21958 assertEquals(b1, b2);
21959 final Matrix ma1 = calibrator.getInitialMa();
21960 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
21961 final Matrix ma2 = new Matrix(3, 3);
21962 calibrator.getInitialMa(ma2);
21963 assertEquals(ma1, ma2);
21964 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21965 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21966 final NEDPosition nedPosition1 = new NEDPosition();
21967 assertTrue(calibrator.getNedPosition(nedPosition1));
21968 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21969 assertSame(calibrator.getMeasurements(), measurements);
21970 assertFalse(calibrator.isCommonAxisUsed());
21971 assertNull(calibrator.getListener());
21972 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21973 assertFalse(calibrator.isReady());
21974 assertFalse(calibrator.isRunning());
21975 assertNull(calibrator.getEstimatedBiases());
21976 assertFalse(calibrator.getEstimatedBiases(null));
21977 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21978 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21979 assertNull(calibrator.getEstimatedBiasFx());
21980 assertNull(calibrator.getEstimatedBiasFy());
21981 assertNull(calibrator.getEstimatedBiasFz());
21982 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21983 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21984 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21985 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21986 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21987 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21988 assertNull(calibrator.getEstimatedMa());
21989 assertNull(calibrator.getEstimatedSx());
21990 assertNull(calibrator.getEstimatedSy());
21991 assertNull(calibrator.getEstimatedSz());
21992 assertNull(calibrator.getEstimatedMxy());
21993 assertNull(calibrator.getEstimatedMxz());
21994 assertNull(calibrator.getEstimatedMyx());
21995 assertNull(calibrator.getEstimatedMyz());
21996 assertNull(calibrator.getEstimatedMzx());
21997 assertNull(calibrator.getEstimatedMzy());
21998 assertNull(calibrator.getEstimatedCovariance());
21999 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22000 }
22001
22002 @Test
22003 public void testConstructor183() throws WrongSizeException {
22004 final Collection<StandardDeviationBodyKinematics> measurements =
22005 Collections.emptyList();
22006
22007 final Matrix ba = generateBa();
22008 final double biasX = ba.getElementAtIndex(0);
22009 final double biasY = ba.getElementAtIndex(1);
22010 final double biasZ = ba.getElementAtIndex(2);
22011
22012 final Matrix ma = generateMaCommonAxis();
22013 final double sx = ma.getElementAt(0, 0);
22014 final double sy = ma.getElementAt(1, 1);
22015 final double sz = ma.getElementAt(2, 2);
22016
22017 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22018 final double latitude = Math.toRadians(
22019 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22020 final double longitude = Math.toRadians(
22021 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22022 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22023 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22024 final NEDVelocity nedVelocity = new NEDVelocity();
22025 final ECEFPosition ecefPosition = new ECEFPosition();
22026 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22027 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22028 ecefPosition, ecefVelocity);
22029
22030 final KnownPositionAccelerometerCalibrator calibrator =
22031 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
22032 biasX, biasY, biasZ, sx, sy, sz, this);
22033
22034
22035 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22036 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22037 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22038 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22039 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22040 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22041 final Acceleration bx2 = new Acceleration(0.0,
22042 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22043 calibrator.getInitialBiasXAsAcceleration(bx2);
22044 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22045 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22046 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22047 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22048 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22049 final Acceleration by2 = new Acceleration(0.0,
22050 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22051 calibrator.getInitialBiasYAsAcceleration(by2);
22052 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22053 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22054 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22055 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22056 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22057 final Acceleration bz2 = new Acceleration(0.0,
22058 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22059 calibrator.getInitialBiasZAsAcceleration(bz2);
22060 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22061 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22062 assertEquals(calibrator.getInitialSx(), sx, 0.0);
22063 assertEquals(calibrator.getInitialSy(), sy, 0.0);
22064 assertEquals(calibrator.getInitialSz(), sz, 0.0);
22065 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22066 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22067 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22068 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22069 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22070 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22071 final double[] bias1 = calibrator.getInitialBias();
22072 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
22073 final double[] bias2 = new double[3];
22074 calibrator.getInitialBias(bias2);
22075 assertArrayEquals(bias1, bias2, 0.0);
22076 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22077 assertEquals(b1, ba);
22078 final Matrix b2 = new Matrix(3, 1);
22079 calibrator.getInitialBiasAsMatrix(b2);
22080 assertEquals(b1, b2);
22081 final Matrix ma1 = calibrator.getInitialMa();
22082 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
22083 final Matrix ma2 = new Matrix(3, 3);
22084 calibrator.getInitialMa(ma2);
22085 assertEquals(ma1, ma2);
22086 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22087 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22088 final NEDPosition nedPosition1 = new NEDPosition();
22089 assertTrue(calibrator.getNedPosition(nedPosition1));
22090 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22091 assertSame(calibrator.getMeasurements(), measurements);
22092 assertFalse(calibrator.isCommonAxisUsed());
22093 assertSame(calibrator.getListener(), this);
22094 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22095 assertFalse(calibrator.isReady());
22096 assertFalse(calibrator.isRunning());
22097 assertNull(calibrator.getEstimatedBiases());
22098 assertFalse(calibrator.getEstimatedBiases(null));
22099 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22100 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22101 assertNull(calibrator.getEstimatedBiasFx());
22102 assertNull(calibrator.getEstimatedBiasFy());
22103 assertNull(calibrator.getEstimatedBiasFz());
22104 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22105 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22106 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22107 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22108 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22109 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22110 assertNull(calibrator.getEstimatedMa());
22111 assertNull(calibrator.getEstimatedSx());
22112 assertNull(calibrator.getEstimatedSy());
22113 assertNull(calibrator.getEstimatedSz());
22114 assertNull(calibrator.getEstimatedMxy());
22115 assertNull(calibrator.getEstimatedMxz());
22116 assertNull(calibrator.getEstimatedMyx());
22117 assertNull(calibrator.getEstimatedMyz());
22118 assertNull(calibrator.getEstimatedMzx());
22119 assertNull(calibrator.getEstimatedMzy());
22120 assertNull(calibrator.getEstimatedCovariance());
22121 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22122 }
22123
22124 @Test
22125 public void testConstructor184() throws WrongSizeException {
22126 final Matrix ba = generateBa();
22127 final double biasX = ba.getElementAtIndex(0);
22128 final double biasY = ba.getElementAtIndex(1);
22129 final double biasZ = ba.getElementAtIndex(2);
22130
22131 final Matrix ma = generateMaCommonAxis();
22132 final double sx = ma.getElementAt(0, 0);
22133 final double sy = ma.getElementAt(1, 1);
22134 final double sz = ma.getElementAt(2, 2);
22135
22136 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22137 final double latitude = Math.toRadians(
22138 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22139 final double longitude = Math.toRadians(
22140 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22141 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22142 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22143 final NEDVelocity nedVelocity = new NEDVelocity();
22144 final ECEFPosition ecefPosition = new ECEFPosition();
22145 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22146 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22147 ecefPosition, ecefVelocity);
22148
22149 final KnownPositionAccelerometerCalibrator calibrator =
22150 new KnownPositionAccelerometerCalibrator(nedPosition,
22151 true, biasX, biasY, biasZ, sx, sy, sz);
22152
22153
22154 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22155 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22156 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22157 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22158 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22159 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22160 final Acceleration bx2 = new Acceleration(0.0,
22161 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22162 calibrator.getInitialBiasXAsAcceleration(bx2);
22163 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22164 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22165 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22166 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22167 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22168 final Acceleration by2 = new Acceleration(0.0,
22169 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22170 calibrator.getInitialBiasYAsAcceleration(by2);
22171 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22172 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22173 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22174 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22175 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22176 final Acceleration bz2 = new Acceleration(0.0,
22177 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22178 calibrator.getInitialBiasZAsAcceleration(bz2);
22179 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22180 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22181 assertEquals(calibrator.getInitialSx(), sx, 0.0);
22182 assertEquals(calibrator.getInitialSy(), sy, 0.0);
22183 assertEquals(calibrator.getInitialSz(), sz, 0.0);
22184 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22185 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22186 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22187 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22188 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22189 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22190 final double[] bias1 = calibrator.getInitialBias();
22191 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
22192 final double[] bias2 = new double[3];
22193 calibrator.getInitialBias(bias2);
22194 assertArrayEquals(bias1, bias2, 0.0);
22195 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22196 assertEquals(b1, ba);
22197 final Matrix b2 = new Matrix(3, 1);
22198 calibrator.getInitialBiasAsMatrix(b2);
22199 assertEquals(b1, b2);
22200 final Matrix ma1 = calibrator.getInitialMa();
22201 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
22202 final Matrix ma2 = new Matrix(3, 3);
22203 calibrator.getInitialMa(ma2);
22204 assertEquals(ma1, ma2);
22205 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22206 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22207 final NEDPosition nedPosition1 = new NEDPosition();
22208 assertTrue(calibrator.getNedPosition(nedPosition1));
22209 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22210 assertNull(calibrator.getMeasurements());
22211 assertTrue(calibrator.isCommonAxisUsed());
22212 assertNull(calibrator.getListener());
22213 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22214 assertFalse(calibrator.isReady());
22215 assertFalse(calibrator.isRunning());
22216 assertNull(calibrator.getEstimatedBiases());
22217 assertFalse(calibrator.getEstimatedBiases(null));
22218 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22219 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22220 assertNull(calibrator.getEstimatedBiasFx());
22221 assertNull(calibrator.getEstimatedBiasFy());
22222 assertNull(calibrator.getEstimatedBiasFz());
22223 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22224 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22225 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22226 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22227 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22228 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22229 assertNull(calibrator.getEstimatedMa());
22230 assertNull(calibrator.getEstimatedSx());
22231 assertNull(calibrator.getEstimatedSy());
22232 assertNull(calibrator.getEstimatedSz());
22233 assertNull(calibrator.getEstimatedMxy());
22234 assertNull(calibrator.getEstimatedMxz());
22235 assertNull(calibrator.getEstimatedMyx());
22236 assertNull(calibrator.getEstimatedMyz());
22237 assertNull(calibrator.getEstimatedMzx());
22238 assertNull(calibrator.getEstimatedMzy());
22239 assertNull(calibrator.getEstimatedCovariance());
22240 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22241 }
22242
22243 @Test
22244 public void testConstructor185() throws WrongSizeException {
22245 final Matrix ba = generateBa();
22246 final double biasX = ba.getElementAtIndex(0);
22247 final double biasY = ba.getElementAtIndex(1);
22248 final double biasZ = ba.getElementAtIndex(2);
22249
22250 final Matrix ma = generateMaCommonAxis();
22251 final double sx = ma.getElementAt(0, 0);
22252 final double sy = ma.getElementAt(1, 1);
22253 final double sz = ma.getElementAt(2, 2);
22254
22255 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22256 final double latitude = Math.toRadians(
22257 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22258 final double longitude = Math.toRadians(
22259 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22260 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22261 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22262 final NEDVelocity nedVelocity = new NEDVelocity();
22263 final ECEFPosition ecefPosition = new ECEFPosition();
22264 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22265 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22266 ecefPosition, ecefVelocity);
22267
22268 final KnownPositionAccelerometerCalibrator calibrator =
22269 new KnownPositionAccelerometerCalibrator(nedPosition,
22270 true, biasX, biasY, biasZ, sx, sy, sz,
22271 this);
22272
22273
22274 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22275 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22276 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22277 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22278 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22279 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22280 final Acceleration bx2 = new Acceleration(0.0,
22281 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22282 calibrator.getInitialBiasXAsAcceleration(bx2);
22283 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22284 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22285 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22286 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22287 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22288 final Acceleration by2 = new Acceleration(0.0,
22289 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22290 calibrator.getInitialBiasYAsAcceleration(by2);
22291 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22292 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22293 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22294 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22295 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22296 final Acceleration bz2 = new Acceleration(0.0,
22297 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22298 calibrator.getInitialBiasZAsAcceleration(bz2);
22299 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22300 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22301 assertEquals(calibrator.getInitialSx(), sx, 0.0);
22302 assertEquals(calibrator.getInitialSy(), sy, 0.0);
22303 assertEquals(calibrator.getInitialSz(), sz, 0.0);
22304 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22305 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22306 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22307 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22308 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22309 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22310 final double[] bias1 = calibrator.getInitialBias();
22311 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
22312 final double[] bias2 = new double[3];
22313 calibrator.getInitialBias(bias2);
22314 assertArrayEquals(bias1, bias2, 0.0);
22315 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22316 assertEquals(b1, ba);
22317 final Matrix b2 = new Matrix(3, 1);
22318 calibrator.getInitialBiasAsMatrix(b2);
22319 assertEquals(b1, b2);
22320 final Matrix ma1 = calibrator.getInitialMa();
22321 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
22322 final Matrix ma2 = new Matrix(3, 3);
22323 calibrator.getInitialMa(ma2);
22324 assertEquals(ma1, ma2);
22325 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22326 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22327 final NEDPosition nedPosition1 = new NEDPosition();
22328 assertTrue(calibrator.getNedPosition(nedPosition1));
22329 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22330 assertNull(calibrator.getMeasurements());
22331 assertTrue(calibrator.isCommonAxisUsed());
22332 assertSame(calibrator.getListener(), this);
22333 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22334 assertFalse(calibrator.isReady());
22335 assertFalse(calibrator.isRunning());
22336 assertNull(calibrator.getEstimatedBiases());
22337 assertFalse(calibrator.getEstimatedBiases(null));
22338 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22339 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22340 assertNull(calibrator.getEstimatedBiasFx());
22341 assertNull(calibrator.getEstimatedBiasFy());
22342 assertNull(calibrator.getEstimatedBiasFz());
22343 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22344 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22345 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22346 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22347 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22348 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22349 assertNull(calibrator.getEstimatedMa());
22350 assertNull(calibrator.getEstimatedSx());
22351 assertNull(calibrator.getEstimatedSy());
22352 assertNull(calibrator.getEstimatedSz());
22353 assertNull(calibrator.getEstimatedMxy());
22354 assertNull(calibrator.getEstimatedMxz());
22355 assertNull(calibrator.getEstimatedMyx());
22356 assertNull(calibrator.getEstimatedMyz());
22357 assertNull(calibrator.getEstimatedMzx());
22358 assertNull(calibrator.getEstimatedMzy());
22359 assertNull(calibrator.getEstimatedCovariance());
22360 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22361 }
22362
22363 @Test
22364 public void testConstructor186() throws WrongSizeException {
22365 final Collection<StandardDeviationBodyKinematics> measurements =
22366 Collections.emptyList();
22367
22368 final Matrix ba = generateBa();
22369 final double biasX = ba.getElementAtIndex(0);
22370 final double biasY = ba.getElementAtIndex(1);
22371 final double biasZ = ba.getElementAtIndex(2);
22372
22373 final Matrix ma = generateMaCommonAxis();
22374 final double sx = ma.getElementAt(0, 0);
22375 final double sy = ma.getElementAt(1, 1);
22376 final double sz = ma.getElementAt(2, 2);
22377
22378 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22379 final double latitude = Math.toRadians(
22380 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22381 final double longitude = Math.toRadians(
22382 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22383 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22384 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22385 final NEDVelocity nedVelocity = new NEDVelocity();
22386 final ECEFPosition ecefPosition = new ECEFPosition();
22387 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22388 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22389 ecefPosition, ecefVelocity);
22390
22391 final KnownPositionAccelerometerCalibrator calibrator =
22392 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
22393 true, biasX, biasY, biasZ, sx, sy, sz);
22394
22395
22396 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22397 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22398 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22399 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22400 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22401 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22402 final Acceleration bx2 = new Acceleration(0.0,
22403 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22404 calibrator.getInitialBiasXAsAcceleration(bx2);
22405 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22406 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22407 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22408 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22409 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22410 final Acceleration by2 = new Acceleration(0.0,
22411 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22412 calibrator.getInitialBiasYAsAcceleration(by2);
22413 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22414 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22415 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22416 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22417 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22418 final Acceleration bz2 = new Acceleration(0.0,
22419 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22420 calibrator.getInitialBiasZAsAcceleration(bz2);
22421 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22422 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22423 assertEquals(calibrator.getInitialSx(), sx, 0.0);
22424 assertEquals(calibrator.getInitialSy(), sy, 0.0);
22425 assertEquals(calibrator.getInitialSz(), sz, 0.0);
22426 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22427 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22428 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22429 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22430 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22431 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22432 final double[] bias1 = calibrator.getInitialBias();
22433 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
22434 final double[] bias2 = new double[3];
22435 calibrator.getInitialBias(bias2);
22436 assertArrayEquals(bias1, bias2, 0.0);
22437 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22438 assertEquals(b1, ba);
22439 final Matrix b2 = new Matrix(3, 1);
22440 calibrator.getInitialBiasAsMatrix(b2);
22441 assertEquals(b1, b2);
22442 final Matrix ma1 = calibrator.getInitialMa();
22443 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
22444 final Matrix ma2 = new Matrix(3, 3);
22445 calibrator.getInitialMa(ma2);
22446 assertEquals(ma1, ma2);
22447 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22448 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22449 final NEDPosition nedPosition1 = new NEDPosition();
22450 assertTrue(calibrator.getNedPosition(nedPosition1));
22451 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22452 assertSame(calibrator.getMeasurements(), measurements);
22453 assertTrue(calibrator.isCommonAxisUsed());
22454 assertNull(calibrator.getListener());
22455 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22456 assertFalse(calibrator.isReady());
22457 assertFalse(calibrator.isRunning());
22458 assertNull(calibrator.getEstimatedBiases());
22459 assertFalse(calibrator.getEstimatedBiases(null));
22460 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22461 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22462 assertNull(calibrator.getEstimatedBiasFx());
22463 assertNull(calibrator.getEstimatedBiasFy());
22464 assertNull(calibrator.getEstimatedBiasFz());
22465 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22466 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22467 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22468 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22469 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22470 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22471 assertNull(calibrator.getEstimatedMa());
22472 assertNull(calibrator.getEstimatedSx());
22473 assertNull(calibrator.getEstimatedSy());
22474 assertNull(calibrator.getEstimatedSz());
22475 assertNull(calibrator.getEstimatedMxy());
22476 assertNull(calibrator.getEstimatedMxz());
22477 assertNull(calibrator.getEstimatedMyx());
22478 assertNull(calibrator.getEstimatedMyz());
22479 assertNull(calibrator.getEstimatedMzx());
22480 assertNull(calibrator.getEstimatedMzy());
22481 assertNull(calibrator.getEstimatedCovariance());
22482 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22483 }
22484
22485 @Test
22486 public void testConstructor187() throws WrongSizeException {
22487 final Collection<StandardDeviationBodyKinematics> measurements =
22488 Collections.emptyList();
22489
22490 final Matrix ba = generateBa();
22491 final double biasX = ba.getElementAtIndex(0);
22492 final double biasY = ba.getElementAtIndex(1);
22493 final double biasZ = ba.getElementAtIndex(2);
22494
22495 final Matrix ma = generateMaCommonAxis();
22496 final double sx = ma.getElementAt(0, 0);
22497 final double sy = ma.getElementAt(1, 1);
22498 final double sz = ma.getElementAt(2, 2);
22499
22500 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22501 final double latitude = Math.toRadians(
22502 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22503 final double longitude = Math.toRadians(
22504 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22505 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22506 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22507 final NEDVelocity nedVelocity = new NEDVelocity();
22508 final ECEFPosition ecefPosition = new ECEFPosition();
22509 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22510 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22511 ecefPosition, ecefVelocity);
22512
22513 final KnownPositionAccelerometerCalibrator calibrator =
22514 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
22515 true, biasX, biasY, biasZ, sx, sy, sz,
22516 this);
22517
22518
22519 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22520 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22521 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22522 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22523 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22524 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22525 final Acceleration bx2 = new Acceleration(0.0,
22526 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22527 calibrator.getInitialBiasXAsAcceleration(bx2);
22528 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22529 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22530 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22531 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22532 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22533 final Acceleration by2 = new Acceleration(0.0,
22534 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22535 calibrator.getInitialBiasYAsAcceleration(by2);
22536 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22537 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22538 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22539 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22540 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22541 final Acceleration bz2 = new Acceleration(0.0,
22542 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22543 calibrator.getInitialBiasZAsAcceleration(bz2);
22544 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22545 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22546 assertEquals(calibrator.getInitialSx(), sx, 0.0);
22547 assertEquals(calibrator.getInitialSy(), sy, 0.0);
22548 assertEquals(calibrator.getInitialSz(), sz, 0.0);
22549 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22550 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22551 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22552 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22553 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22554 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22555 final double[] bias1 = calibrator.getInitialBias();
22556 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
22557 final double[] bias2 = new double[3];
22558 calibrator.getInitialBias(bias2);
22559 assertArrayEquals(bias1, bias2, 0.0);
22560 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22561 assertEquals(b1, ba);
22562 final Matrix b2 = new Matrix(3, 1);
22563 calibrator.getInitialBiasAsMatrix(b2);
22564 assertEquals(b1, b2);
22565 final Matrix ma1 = calibrator.getInitialMa();
22566 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
22567 final Matrix ma2 = new Matrix(3, 3);
22568 calibrator.getInitialMa(ma2);
22569 assertEquals(ma1, ma2);
22570 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22571 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22572 final NEDPosition nedPosition1 = new NEDPosition();
22573 assertTrue(calibrator.getNedPosition(nedPosition1));
22574 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22575 assertSame(calibrator.getMeasurements(), measurements);
22576 assertTrue(calibrator.isCommonAxisUsed());
22577 assertSame(calibrator.getListener(), this);
22578 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22579 assertFalse(calibrator.isReady());
22580 assertFalse(calibrator.isRunning());
22581 assertNull(calibrator.getEstimatedBiases());
22582 assertFalse(calibrator.getEstimatedBiases(null));
22583 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22584 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22585 assertNull(calibrator.getEstimatedBiasFx());
22586 assertNull(calibrator.getEstimatedBiasFy());
22587 assertNull(calibrator.getEstimatedBiasFz());
22588 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22589 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22590 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22591 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22592 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22593 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22594 assertNull(calibrator.getEstimatedMa());
22595 assertNull(calibrator.getEstimatedSx());
22596 assertNull(calibrator.getEstimatedSy());
22597 assertNull(calibrator.getEstimatedSz());
22598 assertNull(calibrator.getEstimatedMxy());
22599 assertNull(calibrator.getEstimatedMxz());
22600 assertNull(calibrator.getEstimatedMyx());
22601 assertNull(calibrator.getEstimatedMyz());
22602 assertNull(calibrator.getEstimatedMzx());
22603 assertNull(calibrator.getEstimatedMzy());
22604 assertNull(calibrator.getEstimatedCovariance());
22605 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22606 }
22607
22608 @Test
22609 public void testConstructor188() throws WrongSizeException {
22610 final Matrix ba = generateBa();
22611 final double biasX = ba.getElementAtIndex(0);
22612 final double biasY = ba.getElementAtIndex(1);
22613 final double biasZ = ba.getElementAtIndex(2);
22614
22615 final Acceleration bx = new Acceleration(biasX,
22616 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22617 final Acceleration by = new Acceleration(biasY,
22618 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22619 final Acceleration bz = new Acceleration(biasZ,
22620 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22621
22622 final Matrix ma = generateMaCommonAxis();
22623 final double sx = ma.getElementAt(0, 0);
22624 final double sy = ma.getElementAt(1, 1);
22625 final double sz = ma.getElementAt(2, 2);
22626
22627 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22628 final double latitude = Math.toRadians(
22629 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22630 final double longitude = Math.toRadians(
22631 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22632 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22633 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22634 final NEDVelocity nedVelocity = new NEDVelocity();
22635 final ECEFPosition ecefPosition = new ECEFPosition();
22636 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22637 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22638 ecefPosition, ecefVelocity);
22639
22640 final KnownPositionAccelerometerCalibrator calibrator =
22641 new KnownPositionAccelerometerCalibrator(nedPosition,
22642 bx, by, bz, sx, sy, sz);
22643
22644
22645 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22646 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22647 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22648 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22649 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22650 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22651 final Acceleration bx2 = new Acceleration(0.0,
22652 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22653 calibrator.getInitialBiasXAsAcceleration(bx2);
22654 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22655 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22656 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22657 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22658 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22659 final Acceleration by2 = new Acceleration(0.0,
22660 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22661 calibrator.getInitialBiasYAsAcceleration(by2);
22662 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22663 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22664 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22665 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22666 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22667 final Acceleration bz2 = new Acceleration(0.0,
22668 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22669 calibrator.getInitialBiasZAsAcceleration(bz2);
22670 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22671 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22672 assertEquals(calibrator.getInitialSx(), sx, 0.0);
22673 assertEquals(calibrator.getInitialSy(), sy, 0.0);
22674 assertEquals(calibrator.getInitialSz(), sz, 0.0);
22675 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22676 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22677 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22678 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22679 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22680 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22681 final double[] bias1 = calibrator.getInitialBias();
22682 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
22683 final double[] bias2 = new double[3];
22684 calibrator.getInitialBias(bias2);
22685 assertArrayEquals(bias1, bias2, 0.0);
22686 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22687 assertEquals(b1, ba);
22688 final Matrix b2 = new Matrix(3, 1);
22689 calibrator.getInitialBiasAsMatrix(b2);
22690 assertEquals(b1, b2);
22691 final Matrix ma1 = calibrator.getInitialMa();
22692 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
22693 final Matrix ma2 = new Matrix(3, 3);
22694 calibrator.getInitialMa(ma2);
22695 assertEquals(ma1, ma2);
22696 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22697 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22698 final NEDPosition nedPosition1 = new NEDPosition();
22699 assertTrue(calibrator.getNedPosition(nedPosition1));
22700 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22701 assertNull(calibrator.getMeasurements());
22702 assertFalse(calibrator.isCommonAxisUsed());
22703 assertNull(calibrator.getListener());
22704 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22705 assertFalse(calibrator.isReady());
22706 assertFalse(calibrator.isRunning());
22707 assertNull(calibrator.getEstimatedBiases());
22708 assertFalse(calibrator.getEstimatedBiases(null));
22709 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22710 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22711 assertNull(calibrator.getEstimatedBiasFx());
22712 assertNull(calibrator.getEstimatedBiasFy());
22713 assertNull(calibrator.getEstimatedBiasFz());
22714 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22715 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22716 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22717 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22718 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22719 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22720 assertNull(calibrator.getEstimatedMa());
22721 assertNull(calibrator.getEstimatedSx());
22722 assertNull(calibrator.getEstimatedSy());
22723 assertNull(calibrator.getEstimatedSz());
22724 assertNull(calibrator.getEstimatedMxy());
22725 assertNull(calibrator.getEstimatedMxz());
22726 assertNull(calibrator.getEstimatedMyx());
22727 assertNull(calibrator.getEstimatedMyz());
22728 assertNull(calibrator.getEstimatedMzx());
22729 assertNull(calibrator.getEstimatedMzy());
22730 assertNull(calibrator.getEstimatedCovariance());
22731 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22732 }
22733
22734 @Test
22735 public void testConstructor189() throws WrongSizeException {
22736 final Matrix ba = generateBa();
22737 final double biasX = ba.getElementAtIndex(0);
22738 final double biasY = ba.getElementAtIndex(1);
22739 final double biasZ = ba.getElementAtIndex(2);
22740
22741 final Acceleration bx = new Acceleration(biasX,
22742 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22743 final Acceleration by = new Acceleration(biasY,
22744 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22745 final Acceleration bz = new Acceleration(biasZ,
22746 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22747
22748 final Matrix ma = generateMaCommonAxis();
22749 final double sx = ma.getElementAt(0, 0);
22750 final double sy = ma.getElementAt(1, 1);
22751 final double sz = ma.getElementAt(2, 2);
22752
22753 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22754 final double latitude = Math.toRadians(
22755 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22756 final double longitude = Math.toRadians(
22757 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22758 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22759 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22760 final NEDVelocity nedVelocity = new NEDVelocity();
22761 final ECEFPosition ecefPosition = new ECEFPosition();
22762 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22763 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22764 ecefPosition, ecefVelocity);
22765
22766 final KnownPositionAccelerometerCalibrator calibrator =
22767 new KnownPositionAccelerometerCalibrator(nedPosition,
22768 bx, by, bz, sx, sy, sz, this);
22769
22770
22771 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22772 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22773 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22774 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22775 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22776 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22777 final Acceleration bx2 = new Acceleration(0.0,
22778 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22779 calibrator.getInitialBiasXAsAcceleration(bx2);
22780 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22781 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22782 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22783 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22784 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22785 final Acceleration by2 = new Acceleration(0.0,
22786 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22787 calibrator.getInitialBiasYAsAcceleration(by2);
22788 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22789 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22790 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22791 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22792 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22793 final Acceleration bz2 = new Acceleration(0.0,
22794 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22795 calibrator.getInitialBiasZAsAcceleration(bz2);
22796 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22797 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22798 assertEquals(calibrator.getInitialSx(), sx, 0.0);
22799 assertEquals(calibrator.getInitialSy(), sy, 0.0);
22800 assertEquals(calibrator.getInitialSz(), sz, 0.0);
22801 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22802 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22803 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22804 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22805 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22806 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22807 final double[] bias1 = calibrator.getInitialBias();
22808 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
22809 final double[] bias2 = new double[3];
22810 calibrator.getInitialBias(bias2);
22811 assertArrayEquals(bias1, bias2, 0.0);
22812 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22813 assertEquals(b1, ba);
22814 final Matrix b2 = new Matrix(3, 1);
22815 calibrator.getInitialBiasAsMatrix(b2);
22816 assertEquals(b1, b2);
22817 final Matrix ma1 = calibrator.getInitialMa();
22818 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
22819 final Matrix ma2 = new Matrix(3, 3);
22820 calibrator.getInitialMa(ma2);
22821 assertEquals(ma1, ma2);
22822 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22823 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22824 final NEDPosition nedPosition1 = new NEDPosition();
22825 assertTrue(calibrator.getNedPosition(nedPosition1));
22826 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22827 assertNull(calibrator.getMeasurements());
22828 assertFalse(calibrator.isCommonAxisUsed());
22829 assertSame(calibrator.getListener(), this);
22830 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22831 assertFalse(calibrator.isReady());
22832 assertFalse(calibrator.isRunning());
22833 assertNull(calibrator.getEstimatedBiases());
22834 assertFalse(calibrator.getEstimatedBiases(null));
22835 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22836 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22837 assertNull(calibrator.getEstimatedBiasFx());
22838 assertNull(calibrator.getEstimatedBiasFy());
22839 assertNull(calibrator.getEstimatedBiasFz());
22840 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22841 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22842 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22843 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22844 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22845 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22846 assertNull(calibrator.getEstimatedMa());
22847 assertNull(calibrator.getEstimatedSx());
22848 assertNull(calibrator.getEstimatedSy());
22849 assertNull(calibrator.getEstimatedSz());
22850 assertNull(calibrator.getEstimatedMxy());
22851 assertNull(calibrator.getEstimatedMxz());
22852 assertNull(calibrator.getEstimatedMyx());
22853 assertNull(calibrator.getEstimatedMyz());
22854 assertNull(calibrator.getEstimatedMzx());
22855 assertNull(calibrator.getEstimatedMzy());
22856 assertNull(calibrator.getEstimatedCovariance());
22857 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22858 }
22859
22860 @Test
22861 public void testConstructor190() throws WrongSizeException {
22862 final Collection<StandardDeviationBodyKinematics> measurements =
22863 Collections.emptyList();
22864
22865 final Matrix ba = generateBa();
22866 final double biasX = ba.getElementAtIndex(0);
22867 final double biasY = ba.getElementAtIndex(1);
22868 final double biasZ = ba.getElementAtIndex(2);
22869
22870 final Acceleration bx = new Acceleration(biasX,
22871 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22872 final Acceleration by = new Acceleration(biasY,
22873 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22874 final Acceleration bz = new Acceleration(biasZ,
22875 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22876
22877 final Matrix ma = generateMaCommonAxis();
22878 final double sx = ma.getElementAt(0, 0);
22879 final double sy = ma.getElementAt(1, 1);
22880 final double sz = ma.getElementAt(2, 2);
22881
22882 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22883 final double latitude = Math.toRadians(
22884 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22885 final double longitude = Math.toRadians(
22886 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22887 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22888 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22889 final NEDVelocity nedVelocity = new NEDVelocity();
22890 final ECEFPosition ecefPosition = new ECEFPosition();
22891 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22892 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22893 ecefPosition, ecefVelocity);
22894
22895 final KnownPositionAccelerometerCalibrator calibrator =
22896 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
22897 bx, by, bz, sx, sy, sz);
22898
22899
22900 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22901 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22902 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22903 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22904 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22905 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22906 final Acceleration bx2 = new Acceleration(0.0,
22907 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22908 calibrator.getInitialBiasXAsAcceleration(bx2);
22909 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22910 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22911 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22912 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22913 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22914 final Acceleration by2 = new Acceleration(0.0,
22915 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22916 calibrator.getInitialBiasYAsAcceleration(by2);
22917 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22918 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22919 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22920 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22921 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22922 final Acceleration bz2 = new Acceleration(0.0,
22923 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22924 calibrator.getInitialBiasZAsAcceleration(bz2);
22925 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22926 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22927 assertEquals(calibrator.getInitialSx(), sx, 0.0);
22928 assertEquals(calibrator.getInitialSy(), sy, 0.0);
22929 assertEquals(calibrator.getInitialSz(), sz, 0.0);
22930 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22931 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22932 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22933 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22934 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22935 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22936 final double[] bias1 = calibrator.getInitialBias();
22937 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
22938 final double[] bias2 = new double[3];
22939 calibrator.getInitialBias(bias2);
22940 assertArrayEquals(bias1, bias2, 0.0);
22941 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22942 assertEquals(b1, ba);
22943 final Matrix b2 = new Matrix(3, 1);
22944 calibrator.getInitialBiasAsMatrix(b2);
22945 assertEquals(b1, b2);
22946 final Matrix ma1 = calibrator.getInitialMa();
22947 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
22948 final Matrix ma2 = new Matrix(3, 3);
22949 calibrator.getInitialMa(ma2);
22950 assertEquals(ma1, ma2);
22951 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22952 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22953 final NEDPosition nedPosition1 = new NEDPosition();
22954 assertTrue(calibrator.getNedPosition(nedPosition1));
22955 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22956 assertSame(calibrator.getMeasurements(), measurements);
22957 assertFalse(calibrator.isCommonAxisUsed());
22958 assertNull(calibrator.getListener());
22959 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22960 assertFalse(calibrator.isReady());
22961 assertFalse(calibrator.isRunning());
22962 assertNull(calibrator.getEstimatedBiases());
22963 assertFalse(calibrator.getEstimatedBiases(null));
22964 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22965 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22966 assertNull(calibrator.getEstimatedBiasFx());
22967 assertNull(calibrator.getEstimatedBiasFy());
22968 assertNull(calibrator.getEstimatedBiasFz());
22969 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22970 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22971 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22972 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22973 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22974 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22975 assertNull(calibrator.getEstimatedMa());
22976 assertNull(calibrator.getEstimatedSx());
22977 assertNull(calibrator.getEstimatedSy());
22978 assertNull(calibrator.getEstimatedSz());
22979 assertNull(calibrator.getEstimatedMxy());
22980 assertNull(calibrator.getEstimatedMxz());
22981 assertNull(calibrator.getEstimatedMyx());
22982 assertNull(calibrator.getEstimatedMyz());
22983 assertNull(calibrator.getEstimatedMzx());
22984 assertNull(calibrator.getEstimatedMzy());
22985 assertNull(calibrator.getEstimatedCovariance());
22986 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22987 }
22988
22989 @Test
22990 public void testConstructor191() throws WrongSizeException {
22991 final Collection<StandardDeviationBodyKinematics> measurements =
22992 Collections.emptyList();
22993
22994 final Matrix ba = generateBa();
22995 final double biasX = ba.getElementAtIndex(0);
22996 final double biasY = ba.getElementAtIndex(1);
22997 final double biasZ = ba.getElementAtIndex(2);
22998
22999 final Acceleration bx = new Acceleration(biasX,
23000 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23001 final Acceleration by = new Acceleration(biasY,
23002 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23003 final Acceleration bz = new Acceleration(biasZ,
23004 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23005
23006 final Matrix ma = generateMaCommonAxis();
23007 final double sx = ma.getElementAt(0, 0);
23008 final double sy = ma.getElementAt(1, 1);
23009 final double sz = ma.getElementAt(2, 2);
23010
23011 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23012 final double latitude = Math.toRadians(
23013 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23014 final double longitude = Math.toRadians(
23015 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23016 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23017 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23018 final NEDVelocity nedVelocity = new NEDVelocity();
23019 final ECEFPosition ecefPosition = new ECEFPosition();
23020 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23021 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23022 ecefPosition, ecefVelocity);
23023
23024 final KnownPositionAccelerometerCalibrator calibrator =
23025 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
23026 bx, by, bz, sx, sy, sz, this);
23027
23028
23029 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23030 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23031 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23032 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23033 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23034 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23035 final Acceleration bx2 = new Acceleration(0.0,
23036 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23037 calibrator.getInitialBiasXAsAcceleration(bx2);
23038 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23039 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23040 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23041 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23042 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23043 final Acceleration by2 = new Acceleration(0.0,
23044 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23045 calibrator.getInitialBiasYAsAcceleration(by2);
23046 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23047 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23048 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23049 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23050 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23051 final Acceleration bz2 = new Acceleration(0.0,
23052 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23053 calibrator.getInitialBiasZAsAcceleration(bz2);
23054 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23055 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23056 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23057 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23058 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23059 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23060 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23061 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23062 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23063 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23064 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23065 final double[] bias1 = calibrator.getInitialBias();
23066 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
23067 final double[] bias2 = new double[3];
23068 calibrator.getInitialBias(bias2);
23069 assertArrayEquals(bias1, bias2, 0.0);
23070 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23071 assertEquals(b1, ba);
23072 final Matrix b2 = new Matrix(3, 1);
23073 calibrator.getInitialBiasAsMatrix(b2);
23074 assertEquals(b1, b2);
23075 final Matrix ma1 = calibrator.getInitialMa();
23076 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
23077 final Matrix ma2 = new Matrix(3, 3);
23078 calibrator.getInitialMa(ma2);
23079 assertEquals(ma1, ma2);
23080 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23081 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23082 final NEDPosition nedPosition1 = new NEDPosition();
23083 assertTrue(calibrator.getNedPosition(nedPosition1));
23084 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23085 assertSame(calibrator.getMeasurements(), measurements);
23086 assertFalse(calibrator.isCommonAxisUsed());
23087 assertSame(calibrator.getListener(), this);
23088 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
23089 assertFalse(calibrator.isReady());
23090 assertFalse(calibrator.isRunning());
23091 assertNull(calibrator.getEstimatedBiases());
23092 assertFalse(calibrator.getEstimatedBiases(null));
23093 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23094 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23095 assertNull(calibrator.getEstimatedBiasFx());
23096 assertNull(calibrator.getEstimatedBiasFy());
23097 assertNull(calibrator.getEstimatedBiasFz());
23098 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23099 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23100 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23101 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23102 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23103 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23104 assertNull(calibrator.getEstimatedMa());
23105 assertNull(calibrator.getEstimatedSx());
23106 assertNull(calibrator.getEstimatedSy());
23107 assertNull(calibrator.getEstimatedSz());
23108 assertNull(calibrator.getEstimatedMxy());
23109 assertNull(calibrator.getEstimatedMxz());
23110 assertNull(calibrator.getEstimatedMyx());
23111 assertNull(calibrator.getEstimatedMyz());
23112 assertNull(calibrator.getEstimatedMzx());
23113 assertNull(calibrator.getEstimatedMzy());
23114 assertNull(calibrator.getEstimatedCovariance());
23115 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23116 }
23117
23118 @Test
23119 public void testConstructor192() throws WrongSizeException {
23120 final Matrix ba = generateBa();
23121 final double biasX = ba.getElementAtIndex(0);
23122 final double biasY = ba.getElementAtIndex(1);
23123 final double biasZ = ba.getElementAtIndex(2);
23124
23125 final Acceleration bx = new Acceleration(biasX,
23126 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23127 final Acceleration by = new Acceleration(biasY,
23128 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23129 final Acceleration bz = new Acceleration(biasZ,
23130 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23131
23132 final Matrix ma = generateMaCommonAxis();
23133 final double sx = ma.getElementAt(0, 0);
23134 final double sy = ma.getElementAt(1, 1);
23135 final double sz = ma.getElementAt(2, 2);
23136
23137 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23138 final double latitude = Math.toRadians(
23139 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23140 final double longitude = Math.toRadians(
23141 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23142 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23143 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23144 final NEDVelocity nedVelocity = new NEDVelocity();
23145 final ECEFPosition ecefPosition = new ECEFPosition();
23146 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23147 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23148 ecefPosition, ecefVelocity);
23149
23150 final KnownPositionAccelerometerCalibrator calibrator =
23151 new KnownPositionAccelerometerCalibrator(nedPosition,
23152 true, bx, by, bz, sx, sy, sz);
23153
23154
23155 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23156 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23157 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23158 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23159 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23160 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23161 final Acceleration bx2 = new Acceleration(0.0,
23162 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23163 calibrator.getInitialBiasXAsAcceleration(bx2);
23164 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23165 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23166 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23167 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23168 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23169 final Acceleration by2 = new Acceleration(0.0,
23170 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23171 calibrator.getInitialBiasYAsAcceleration(by2);
23172 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23173 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23174 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23175 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23176 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23177 final Acceleration bz2 = new Acceleration(0.0,
23178 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23179 calibrator.getInitialBiasZAsAcceleration(bz2);
23180 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23181 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23182 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23183 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23184 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23185 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23186 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23187 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23188 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23189 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23190 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23191 final double[] bias1 = calibrator.getInitialBias();
23192 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
23193 final double[] bias2 = new double[3];
23194 calibrator.getInitialBias(bias2);
23195 assertArrayEquals(bias1, bias2, 0.0);
23196 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23197 assertEquals(b1, ba);
23198 final Matrix b2 = new Matrix(3, 1);
23199 calibrator.getInitialBiasAsMatrix(b2);
23200 assertEquals(b1, b2);
23201 final Matrix ma1 = calibrator.getInitialMa();
23202 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
23203 final Matrix ma2 = new Matrix(3, 3);
23204 calibrator.getInitialMa(ma2);
23205 assertEquals(ma1, ma2);
23206 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23207 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23208 final NEDPosition nedPosition1 = new NEDPosition();
23209 assertTrue(calibrator.getNedPosition(nedPosition1));
23210 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23211 assertNull(calibrator.getMeasurements());
23212 assertTrue(calibrator.isCommonAxisUsed());
23213 assertNull(calibrator.getListener());
23214 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23215 assertFalse(calibrator.isReady());
23216 assertFalse(calibrator.isRunning());
23217 assertNull(calibrator.getEstimatedBiases());
23218 assertFalse(calibrator.getEstimatedBiases(null));
23219 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23220 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23221 assertNull(calibrator.getEstimatedBiasFx());
23222 assertNull(calibrator.getEstimatedBiasFy());
23223 assertNull(calibrator.getEstimatedBiasFz());
23224 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23225 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23226 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23227 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23228 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23229 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23230 assertNull(calibrator.getEstimatedMa());
23231 assertNull(calibrator.getEstimatedSx());
23232 assertNull(calibrator.getEstimatedSy());
23233 assertNull(calibrator.getEstimatedSz());
23234 assertNull(calibrator.getEstimatedMxy());
23235 assertNull(calibrator.getEstimatedMxz());
23236 assertNull(calibrator.getEstimatedMyx());
23237 assertNull(calibrator.getEstimatedMyz());
23238 assertNull(calibrator.getEstimatedMzx());
23239 assertNull(calibrator.getEstimatedMzy());
23240 assertNull(calibrator.getEstimatedCovariance());
23241 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23242 }
23243
23244 @Test
23245 public void testConstructor193() throws WrongSizeException {
23246 final Matrix ba = generateBa();
23247 final double biasX = ba.getElementAtIndex(0);
23248 final double biasY = ba.getElementAtIndex(1);
23249 final double biasZ = ba.getElementAtIndex(2);
23250
23251 final Acceleration bx = new Acceleration(biasX,
23252 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23253 final Acceleration by = new Acceleration(biasY,
23254 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23255 final Acceleration bz = new Acceleration(biasZ,
23256 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23257
23258 final Matrix ma = generateMaCommonAxis();
23259 final double sx = ma.getElementAt(0, 0);
23260 final double sy = ma.getElementAt(1, 1);
23261 final double sz = ma.getElementAt(2, 2);
23262
23263 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23264 final double latitude = Math.toRadians(
23265 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23266 final double longitude = Math.toRadians(
23267 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23268 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23269 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23270 final NEDVelocity nedVelocity = new NEDVelocity();
23271 final ECEFPosition ecefPosition = new ECEFPosition();
23272 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23273 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23274 ecefPosition, ecefVelocity);
23275
23276 final KnownPositionAccelerometerCalibrator calibrator =
23277 new KnownPositionAccelerometerCalibrator(nedPosition,
23278 true, bx, by, bz, sx, sy, sz, this);
23279
23280
23281 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23282 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23283 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23284 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23285 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23286 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23287 final Acceleration bx2 = new Acceleration(0.0,
23288 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23289 calibrator.getInitialBiasXAsAcceleration(bx2);
23290 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23291 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23292 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23293 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23294 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23295 final Acceleration by2 = new Acceleration(0.0,
23296 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23297 calibrator.getInitialBiasYAsAcceleration(by2);
23298 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23299 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23300 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23301 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23302 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23303 final Acceleration bz2 = new Acceleration(0.0,
23304 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23305 calibrator.getInitialBiasZAsAcceleration(bz2);
23306 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23307 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23308 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23309 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23310 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23311 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23312 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23313 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23314 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23315 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23316 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23317 final double[] bias1 = calibrator.getInitialBias();
23318 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
23319 final double[] bias2 = new double[3];
23320 calibrator.getInitialBias(bias2);
23321 assertArrayEquals(bias1, bias2, 0.0);
23322 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23323 assertEquals(b1, ba);
23324 final Matrix b2 = new Matrix(3, 1);
23325 calibrator.getInitialBiasAsMatrix(b2);
23326 assertEquals(b1, b2);
23327 final Matrix ma1 = calibrator.getInitialMa();
23328 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
23329 final Matrix ma2 = new Matrix(3, 3);
23330 calibrator.getInitialMa(ma2);
23331 assertEquals(ma1, ma2);
23332 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23333 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23334 final NEDPosition nedPosition1 = new NEDPosition();
23335 assertTrue(calibrator.getNedPosition(nedPosition1));
23336 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23337 assertNull(calibrator.getMeasurements());
23338 assertTrue(calibrator.isCommonAxisUsed());
23339 assertSame(calibrator.getListener(), this);
23340 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23341 assertFalse(calibrator.isReady());
23342 assertFalse(calibrator.isRunning());
23343 assertNull(calibrator.getEstimatedBiases());
23344 assertFalse(calibrator.getEstimatedBiases(null));
23345 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23346 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23347 assertNull(calibrator.getEstimatedBiasFx());
23348 assertNull(calibrator.getEstimatedBiasFy());
23349 assertNull(calibrator.getEstimatedBiasFz());
23350 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23351 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23352 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23353 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23354 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23355 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23356 assertNull(calibrator.getEstimatedMa());
23357 assertNull(calibrator.getEstimatedSx());
23358 assertNull(calibrator.getEstimatedSy());
23359 assertNull(calibrator.getEstimatedSz());
23360 assertNull(calibrator.getEstimatedMxy());
23361 assertNull(calibrator.getEstimatedMxz());
23362 assertNull(calibrator.getEstimatedMyx());
23363 assertNull(calibrator.getEstimatedMyz());
23364 assertNull(calibrator.getEstimatedMzx());
23365 assertNull(calibrator.getEstimatedMzy());
23366 assertNull(calibrator.getEstimatedCovariance());
23367 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23368 }
23369
23370 @Test
23371 public void testConstructor194() throws WrongSizeException {
23372 final Collection<StandardDeviationBodyKinematics> measurements =
23373 Collections.emptyList();
23374
23375 final Matrix ba = generateBa();
23376 final double biasX = ba.getElementAtIndex(0);
23377 final double biasY = ba.getElementAtIndex(1);
23378 final double biasZ = ba.getElementAtIndex(2);
23379
23380 final Acceleration bx = new Acceleration(biasX,
23381 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23382 final Acceleration by = new Acceleration(biasY,
23383 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23384 final Acceleration bz = new Acceleration(biasZ,
23385 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23386
23387 final Matrix ma = generateMaCommonAxis();
23388 final double sx = ma.getElementAt(0, 0);
23389 final double sy = ma.getElementAt(1, 1);
23390 final double sz = ma.getElementAt(2, 2);
23391
23392 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23393 final double latitude = Math.toRadians(
23394 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23395 final double longitude = Math.toRadians(
23396 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23397 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23398 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23399 final NEDVelocity nedVelocity = new NEDVelocity();
23400 final ECEFPosition ecefPosition = new ECEFPosition();
23401 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23402 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23403 ecefPosition, ecefVelocity);
23404
23405 final KnownPositionAccelerometerCalibrator calibrator =
23406 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
23407 true, bx, by, bz, sx, sy, sz);
23408
23409
23410 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23411 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23412 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23413 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23414 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23415 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23416 final Acceleration bx2 = new Acceleration(0.0,
23417 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23418 calibrator.getInitialBiasXAsAcceleration(bx2);
23419 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23420 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23421 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23422 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23423 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23424 final Acceleration by2 = new Acceleration(0.0,
23425 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23426 calibrator.getInitialBiasYAsAcceleration(by2);
23427 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23428 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23429 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23430 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23431 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23432 final Acceleration bz2 = new Acceleration(0.0,
23433 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23434 calibrator.getInitialBiasZAsAcceleration(bz2);
23435 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23436 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23437 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23438 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23439 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23440 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23441 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23442 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23443 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23444 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23445 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23446 final double[] bias1 = calibrator.getInitialBias();
23447 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
23448 final double[] bias2 = new double[3];
23449 calibrator.getInitialBias(bias2);
23450 assertArrayEquals(bias1, bias2, 0.0);
23451 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23452 assertEquals(b1, ba);
23453 final Matrix b2 = new Matrix(3, 1);
23454 calibrator.getInitialBiasAsMatrix(b2);
23455 assertEquals(b1, b2);
23456 final Matrix ma1 = calibrator.getInitialMa();
23457 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
23458 final Matrix ma2 = new Matrix(3, 3);
23459 calibrator.getInitialMa(ma2);
23460 assertEquals(ma1, ma2);
23461 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23462 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23463 final NEDPosition nedPosition1 = new NEDPosition();
23464 assertTrue(calibrator.getNedPosition(nedPosition1));
23465 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23466 assertSame(calibrator.getMeasurements(), measurements);
23467 assertTrue(calibrator.isCommonAxisUsed());
23468 assertNull(calibrator.getListener());
23469 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23470 assertFalse(calibrator.isReady());
23471 assertFalse(calibrator.isRunning());
23472 assertNull(calibrator.getEstimatedBiases());
23473 assertFalse(calibrator.getEstimatedBiases(null));
23474 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23475 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23476 assertNull(calibrator.getEstimatedBiasFx());
23477 assertNull(calibrator.getEstimatedBiasFy());
23478 assertNull(calibrator.getEstimatedBiasFz());
23479 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23480 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23481 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23482 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23483 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23484 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23485 assertNull(calibrator.getEstimatedMa());
23486 assertNull(calibrator.getEstimatedSx());
23487 assertNull(calibrator.getEstimatedSy());
23488 assertNull(calibrator.getEstimatedSz());
23489 assertNull(calibrator.getEstimatedMxy());
23490 assertNull(calibrator.getEstimatedMxz());
23491 assertNull(calibrator.getEstimatedMyx());
23492 assertNull(calibrator.getEstimatedMyz());
23493 assertNull(calibrator.getEstimatedMzx());
23494 assertNull(calibrator.getEstimatedMzy());
23495 assertNull(calibrator.getEstimatedCovariance());
23496 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23497 }
23498
23499 @Test
23500 public void testConstructor195() throws WrongSizeException {
23501 final Collection<StandardDeviationBodyKinematics> measurements =
23502 Collections.emptyList();
23503
23504 final Matrix ba = generateBa();
23505 final double biasX = ba.getElementAtIndex(0);
23506 final double biasY = ba.getElementAtIndex(1);
23507 final double biasZ = ba.getElementAtIndex(2);
23508
23509 final Acceleration bx = new Acceleration(biasX,
23510 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23511 final Acceleration by = new Acceleration(biasY,
23512 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23513 final Acceleration bz = new Acceleration(biasZ,
23514 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23515
23516 final Matrix ma = generateMaCommonAxis();
23517 final double sx = ma.getElementAt(0, 0);
23518 final double sy = ma.getElementAt(1, 1);
23519 final double sz = ma.getElementAt(2, 2);
23520
23521 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23522 final double latitude = Math.toRadians(
23523 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23524 final double longitude = Math.toRadians(
23525 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23526 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23527 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23528 final NEDVelocity nedVelocity = new NEDVelocity();
23529 final ECEFPosition ecefPosition = new ECEFPosition();
23530 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23531 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23532 ecefPosition, ecefVelocity);
23533
23534 final KnownPositionAccelerometerCalibrator calibrator =
23535 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
23536 true, bx, by, bz, sx, sy, sz, this);
23537
23538
23539 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23540 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23541 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23542 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23543 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23544 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23545 final Acceleration bx2 = new Acceleration(0.0,
23546 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23547 calibrator.getInitialBiasXAsAcceleration(bx2);
23548 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23549 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23550 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23551 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23552 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23553 final Acceleration by2 = new Acceleration(0.0,
23554 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23555 calibrator.getInitialBiasYAsAcceleration(by2);
23556 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23557 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23558 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23559 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23560 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23561 final Acceleration bz2 = new Acceleration(0.0,
23562 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23563 calibrator.getInitialBiasZAsAcceleration(bz2);
23564 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23565 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23566 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23567 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23568 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23569 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23570 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23571 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23572 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23573 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23574 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23575 final double[] bias1 = calibrator.getInitialBias();
23576 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
23577 final double[] bias2 = new double[3];
23578 calibrator.getInitialBias(bias2);
23579 assertArrayEquals(bias1, bias2, 0.0);
23580 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23581 assertEquals(b1, ba);
23582 final Matrix b2 = new Matrix(3, 1);
23583 calibrator.getInitialBiasAsMatrix(b2);
23584 assertEquals(b1, b2);
23585 final Matrix ma1 = calibrator.getInitialMa();
23586 assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
23587 final Matrix ma2 = new Matrix(3, 3);
23588 calibrator.getInitialMa(ma2);
23589 assertEquals(ma1, ma2);
23590 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23591 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23592 final NEDPosition nedPosition1 = new NEDPosition();
23593 assertTrue(calibrator.getNedPosition(nedPosition1));
23594 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23595 assertSame(calibrator.getMeasurements(), measurements);
23596 assertTrue(calibrator.isCommonAxisUsed());
23597 assertSame(calibrator.getListener(), this);
23598 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23599 assertFalse(calibrator.isReady());
23600 assertFalse(calibrator.isRunning());
23601 assertNull(calibrator.getEstimatedBiases());
23602 assertFalse(calibrator.getEstimatedBiases(null));
23603 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23604 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23605 assertNull(calibrator.getEstimatedBiasFx());
23606 assertNull(calibrator.getEstimatedBiasFy());
23607 assertNull(calibrator.getEstimatedBiasFz());
23608 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23609 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23610 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23611 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23612 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23613 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23614 assertNull(calibrator.getEstimatedMa());
23615 assertNull(calibrator.getEstimatedSx());
23616 assertNull(calibrator.getEstimatedSy());
23617 assertNull(calibrator.getEstimatedSz());
23618 assertNull(calibrator.getEstimatedMxy());
23619 assertNull(calibrator.getEstimatedMxz());
23620 assertNull(calibrator.getEstimatedMyx());
23621 assertNull(calibrator.getEstimatedMyz());
23622 assertNull(calibrator.getEstimatedMzx());
23623 assertNull(calibrator.getEstimatedMzy());
23624 assertNull(calibrator.getEstimatedCovariance());
23625 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23626 }
23627
23628 @Test
23629 public void testConstructor196() throws WrongSizeException {
23630 final Matrix ba = generateBa();
23631 final double biasX = ba.getElementAtIndex(0);
23632 final double biasY = ba.getElementAtIndex(1);
23633 final double biasZ = ba.getElementAtIndex(2);
23634
23635 final Matrix ma = generateMaCommonAxis();
23636 final double sx = ma.getElementAt(0, 0);
23637 final double sy = ma.getElementAt(1, 1);
23638 final double sz = ma.getElementAt(2, 2);
23639 final double mxy = ma.getElementAt(0, 1);
23640 final double mxz = ma.getElementAt(0, 2);
23641 final double myx = ma.getElementAt(1, 0);
23642 final double myz = ma.getElementAt(1, 2);
23643 final double mzx = ma.getElementAt(2, 0);
23644 final double mzy = ma.getElementAt(2, 1);
23645
23646 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23647 final double latitude = Math.toRadians(
23648 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23649 final double longitude = Math.toRadians(
23650 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23651 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23652 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23653 final NEDVelocity nedVelocity = new NEDVelocity();
23654 final ECEFPosition ecefPosition = new ECEFPosition();
23655 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23656 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23657 ecefPosition, ecefVelocity);
23658
23659 final KnownPositionAccelerometerCalibrator calibrator =
23660 new KnownPositionAccelerometerCalibrator(nedPosition,
23661 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
23662 myx, myz, mzx, mzy);
23663
23664
23665 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23666 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23667 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23668 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23669 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23670 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23671 final Acceleration bx2 = new Acceleration(0.0,
23672 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23673 calibrator.getInitialBiasXAsAcceleration(bx2);
23674 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23675 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23676 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23677 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23678 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23679 final Acceleration by2 = new Acceleration(0.0,
23680 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23681 calibrator.getInitialBiasYAsAcceleration(by2);
23682 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23683 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23684 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23685 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23686 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23687 final Acceleration bz2 = new Acceleration(0.0,
23688 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23689 calibrator.getInitialBiasZAsAcceleration(bz2);
23690 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23691 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23692 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23693 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23694 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23695 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
23696 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
23697 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
23698 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
23699 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
23700 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
23701 final double[] bias1 = calibrator.getInitialBias();
23702 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
23703 final double[] bias2 = new double[3];
23704 calibrator.getInitialBias(bias2);
23705 assertArrayEquals(bias1, bias2, 0.0);
23706 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23707 assertEquals(b1, ba);
23708 final Matrix b2 = new Matrix(3, 1);
23709 calibrator.getInitialBiasAsMatrix(b2);
23710 assertEquals(b1, b2);
23711 final Matrix ma1 = new Matrix(3, 3);
23712 ma1.setSubmatrix(0, 0,
23713 2, 2,
23714 new double[]{ sx, myx, mzx,
23715 mxy, sy, mzy,
23716 mxz, myz, sz});
23717 assertEquals(calibrator.getInitialMa(), ma1);
23718 final Matrix ma2 = new Matrix(3, 3);
23719 calibrator.getInitialMa(ma2);
23720 assertEquals(ma1, ma2);
23721 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23722 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23723 final NEDPosition nedPosition1 = new NEDPosition();
23724 assertTrue(calibrator.getNedPosition(nedPosition1));
23725 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23726 assertNull(calibrator.getMeasurements());
23727 assertFalse(calibrator.isCommonAxisUsed());
23728 assertNull(calibrator.getListener());
23729 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
23730 assertFalse(calibrator.isReady());
23731 assertFalse(calibrator.isRunning());
23732 assertNull(calibrator.getEstimatedBiases());
23733 assertFalse(calibrator.getEstimatedBiases(null));
23734 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23735 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23736 assertNull(calibrator.getEstimatedBiasFx());
23737 assertNull(calibrator.getEstimatedBiasFy());
23738 assertNull(calibrator.getEstimatedBiasFz());
23739 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23740 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23741 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23742 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23743 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23744 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23745 assertNull(calibrator.getEstimatedMa());
23746 assertNull(calibrator.getEstimatedSx());
23747 assertNull(calibrator.getEstimatedSy());
23748 assertNull(calibrator.getEstimatedSz());
23749 assertNull(calibrator.getEstimatedMxy());
23750 assertNull(calibrator.getEstimatedMxz());
23751 assertNull(calibrator.getEstimatedMyx());
23752 assertNull(calibrator.getEstimatedMyz());
23753 assertNull(calibrator.getEstimatedMzx());
23754 assertNull(calibrator.getEstimatedMzy());
23755 assertNull(calibrator.getEstimatedCovariance());
23756 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23757 }
23758
23759 @Test
23760 public void testConstructor197() throws WrongSizeException {
23761 final Collection<StandardDeviationBodyKinematics> measurements =
23762 Collections.emptyList();
23763
23764 final Matrix ba = generateBa();
23765 final double biasX = ba.getElementAtIndex(0);
23766 final double biasY = ba.getElementAtIndex(1);
23767 final double biasZ = ba.getElementAtIndex(2);
23768
23769 final Matrix ma = generateMaCommonAxis();
23770 final double sx = ma.getElementAt(0, 0);
23771 final double sy = ma.getElementAt(1, 1);
23772 final double sz = ma.getElementAt(2, 2);
23773 final double mxy = ma.getElementAt(0, 1);
23774 final double mxz = ma.getElementAt(0, 2);
23775 final double myx = ma.getElementAt(1, 0);
23776 final double myz = ma.getElementAt(1, 2);
23777 final double mzx = ma.getElementAt(2, 0);
23778 final double mzy = ma.getElementAt(2, 1);
23779
23780 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23781 final double latitude = Math.toRadians(
23782 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23783 final double longitude = Math.toRadians(
23784 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23785 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23786 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23787 final NEDVelocity nedVelocity = new NEDVelocity();
23788 final ECEFPosition ecefPosition = new ECEFPosition();
23789 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23790 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23791 ecefPosition, ecefVelocity);
23792
23793 final KnownPositionAccelerometerCalibrator calibrator =
23794 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
23795 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
23796 myx, myz, mzx, mzy);
23797
23798
23799 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23800 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23801 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23802 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23803 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23804 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23805 final Acceleration bx2 = new Acceleration(0.0,
23806 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23807 calibrator.getInitialBiasXAsAcceleration(bx2);
23808 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23809 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23810 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23811 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23812 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23813 final Acceleration by2 = new Acceleration(0.0,
23814 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23815 calibrator.getInitialBiasYAsAcceleration(by2);
23816 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23817 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23818 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23819 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23820 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23821 final Acceleration bz2 = new Acceleration(0.0,
23822 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23823 calibrator.getInitialBiasZAsAcceleration(bz2);
23824 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23825 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23826 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23827 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23828 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23829 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
23830 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
23831 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
23832 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
23833 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
23834 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
23835 final double[] bias1 = calibrator.getInitialBias();
23836 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
23837 final double[] bias2 = new double[3];
23838 calibrator.getInitialBias(bias2);
23839 assertArrayEquals(bias1, bias2, 0.0);
23840 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23841 assertEquals(b1, ba);
23842 final Matrix b2 = new Matrix(3, 1);
23843 calibrator.getInitialBiasAsMatrix(b2);
23844 assertEquals(b1, b2);
23845 final Matrix ma1 = new Matrix(3, 3);
23846 ma1.setSubmatrix(0, 0,
23847 2, 2,
23848 new double[]{ sx, myx, mzx,
23849 mxy, sy, mzy,
23850 mxz, myz, sz});
23851 assertEquals(calibrator.getInitialMa(), ma1);
23852 final Matrix ma2 = new Matrix(3, 3);
23853 calibrator.getInitialMa(ma2);
23854 assertEquals(ma1, ma2);
23855 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23856 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23857 final NEDPosition nedPosition1 = new NEDPosition();
23858 assertTrue(calibrator.getNedPosition(nedPosition1));
23859 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23860 assertSame(calibrator.getMeasurements(), measurements);
23861 assertFalse(calibrator.isCommonAxisUsed());
23862 assertNull(calibrator.getListener());
23863 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
23864 assertFalse(calibrator.isReady());
23865 assertFalse(calibrator.isRunning());
23866 assertNull(calibrator.getEstimatedBiases());
23867 assertFalse(calibrator.getEstimatedBiases(null));
23868 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23869 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23870 assertNull(calibrator.getEstimatedBiasFx());
23871 assertNull(calibrator.getEstimatedBiasFy());
23872 assertNull(calibrator.getEstimatedBiasFz());
23873 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23874 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23875 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23876 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23877 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23878 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23879 assertNull(calibrator.getEstimatedMa());
23880 assertNull(calibrator.getEstimatedSx());
23881 assertNull(calibrator.getEstimatedSy());
23882 assertNull(calibrator.getEstimatedSz());
23883 assertNull(calibrator.getEstimatedMxy());
23884 assertNull(calibrator.getEstimatedMxz());
23885 assertNull(calibrator.getEstimatedMyx());
23886 assertNull(calibrator.getEstimatedMyz());
23887 assertNull(calibrator.getEstimatedMzx());
23888 assertNull(calibrator.getEstimatedMzy());
23889 assertNull(calibrator.getEstimatedCovariance());
23890 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23891 }
23892
23893 @Test
23894 public void testConstructor198() throws WrongSizeException {
23895 final Collection<StandardDeviationBodyKinematics> measurements =
23896 Collections.emptyList();
23897
23898 final Matrix ba = generateBa();
23899 final double biasX = ba.getElementAtIndex(0);
23900 final double biasY = ba.getElementAtIndex(1);
23901 final double biasZ = ba.getElementAtIndex(2);
23902
23903 final Matrix ma = generateMaCommonAxis();
23904 final double sx = ma.getElementAt(0, 0);
23905 final double sy = ma.getElementAt(1, 1);
23906 final double sz = ma.getElementAt(2, 2);
23907 final double mxy = ma.getElementAt(0, 1);
23908 final double mxz = ma.getElementAt(0, 2);
23909 final double myx = ma.getElementAt(1, 0);
23910 final double myz = ma.getElementAt(1, 2);
23911 final double mzx = ma.getElementAt(2, 0);
23912 final double mzy = ma.getElementAt(2, 1);
23913
23914 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23915 final double latitude = Math.toRadians(
23916 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23917 final double longitude = Math.toRadians(
23918 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23919 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23920 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23921 final NEDVelocity nedVelocity = new NEDVelocity();
23922 final ECEFPosition ecefPosition = new ECEFPosition();
23923 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23924 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23925 ecefPosition, ecefVelocity);
23926
23927 final KnownPositionAccelerometerCalibrator calibrator =
23928 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
23929 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
23930 myx, myz, mzx, mzy, this);
23931
23932
23933 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23934 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23935 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23936 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23937 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23938 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23939 final Acceleration bx2 = new Acceleration(0.0,
23940 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23941 calibrator.getInitialBiasXAsAcceleration(bx2);
23942 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23943 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23944 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23945 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23946 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23947 final Acceleration by2 = new Acceleration(0.0,
23948 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23949 calibrator.getInitialBiasYAsAcceleration(by2);
23950 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23951 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23952 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23953 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23954 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23955 final Acceleration bz2 = new Acceleration(0.0,
23956 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23957 calibrator.getInitialBiasZAsAcceleration(bz2);
23958 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23959 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23960 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23961 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23962 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23963 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
23964 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
23965 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
23966 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
23967 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
23968 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
23969 final double[] bias1 = calibrator.getInitialBias();
23970 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
23971 final double[] bias2 = new double[3];
23972 calibrator.getInitialBias(bias2);
23973 assertArrayEquals(bias1, bias2, 0.0);
23974 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23975 assertEquals(b1, ba);
23976 final Matrix b2 = new Matrix(3, 1);
23977 calibrator.getInitialBiasAsMatrix(b2);
23978 assertEquals(b1, b2);
23979 final Matrix ma1 = new Matrix(3, 3);
23980 ma1.setSubmatrix(0, 0,
23981 2, 2,
23982 new double[]{ sx, myx, mzx,
23983 mxy, sy, mzy,
23984 mxz, myz, sz});
23985 assertEquals(calibrator.getInitialMa(), ma1);
23986 final Matrix ma2 = new Matrix(3, 3);
23987 calibrator.getInitialMa(ma2);
23988 assertEquals(ma1, ma2);
23989 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23990 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23991 final NEDPosition nedPosition1 = new NEDPosition();
23992 assertTrue(calibrator.getNedPosition(nedPosition1));
23993 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23994 assertSame(calibrator.getMeasurements(), measurements);
23995 assertFalse(calibrator.isCommonAxisUsed());
23996 assertSame(calibrator.getListener(), this);
23997 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
23998 assertFalse(calibrator.isReady());
23999 assertFalse(calibrator.isRunning());
24000 assertNull(calibrator.getEstimatedBiases());
24001 assertFalse(calibrator.getEstimatedBiases(null));
24002 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24003 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24004 assertNull(calibrator.getEstimatedBiasFx());
24005 assertNull(calibrator.getEstimatedBiasFy());
24006 assertNull(calibrator.getEstimatedBiasFz());
24007 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24008 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24009 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24010 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24011 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24012 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24013 assertNull(calibrator.getEstimatedMa());
24014 assertNull(calibrator.getEstimatedSx());
24015 assertNull(calibrator.getEstimatedSy());
24016 assertNull(calibrator.getEstimatedSz());
24017 assertNull(calibrator.getEstimatedMxy());
24018 assertNull(calibrator.getEstimatedMxz());
24019 assertNull(calibrator.getEstimatedMyx());
24020 assertNull(calibrator.getEstimatedMyz());
24021 assertNull(calibrator.getEstimatedMzx());
24022 assertNull(calibrator.getEstimatedMzy());
24023 assertNull(calibrator.getEstimatedCovariance());
24024 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24025 }
24026
24027 @Test
24028 public void testConstructor199() throws WrongSizeException {
24029 final Matrix ba = generateBa();
24030 final double biasX = ba.getElementAtIndex(0);
24031 final double biasY = ba.getElementAtIndex(1);
24032 final double biasZ = ba.getElementAtIndex(2);
24033
24034 final Matrix ma = generateMaCommonAxis();
24035 final double sx = ma.getElementAt(0, 0);
24036 final double sy = ma.getElementAt(1, 1);
24037 final double sz = ma.getElementAt(2, 2);
24038 final double mxy = ma.getElementAt(0, 1);
24039 final double mxz = ma.getElementAt(0, 2);
24040 final double myx = ma.getElementAt(1, 0);
24041 final double myz = ma.getElementAt(1, 2);
24042 final double mzx = ma.getElementAt(2, 0);
24043 final double mzy = ma.getElementAt(2, 1);
24044
24045 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24046 final double latitude = Math.toRadians(
24047 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24048 final double longitude = Math.toRadians(
24049 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24050 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24051 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24052 final NEDVelocity nedVelocity = new NEDVelocity();
24053 final ECEFPosition ecefPosition = new ECEFPosition();
24054 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24055 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24056 ecefPosition, ecefVelocity);
24057
24058 final KnownPositionAccelerometerCalibrator calibrator =
24059 new KnownPositionAccelerometerCalibrator(nedPosition,
24060 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
24061 myx, myz, mzx, mzy);
24062
24063
24064 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24065 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24066 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24067 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24068 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24069 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24070 final Acceleration bx2 = new Acceleration(0.0,
24071 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24072 calibrator.getInitialBiasXAsAcceleration(bx2);
24073 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24074 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24075 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24076 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24077 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24078 final Acceleration by2 = new Acceleration(0.0,
24079 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24080 calibrator.getInitialBiasYAsAcceleration(by2);
24081 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24082 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24083 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24084 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24085 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24086 final Acceleration bz2 = new Acceleration(0.0,
24087 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24088 calibrator.getInitialBiasZAsAcceleration(bz2);
24089 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24090 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24091 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24092 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24093 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24094 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
24095 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
24096 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
24097 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
24098 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
24099 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
24100 final double[] bias1 = calibrator.getInitialBias();
24101 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
24102 final double[] bias2 = new double[3];
24103 calibrator.getInitialBias(bias2);
24104 assertArrayEquals(bias1, bias2, 0.0);
24105 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24106 assertEquals(b1, ba);
24107 final Matrix b2 = new Matrix(3, 1);
24108 calibrator.getInitialBiasAsMatrix(b2);
24109 assertEquals(b1, b2);
24110 final Matrix ma1 = new Matrix(3, 3);
24111 ma1.setSubmatrix(0, 0,
24112 2, 2,
24113 new double[]{ sx, myx, mzx,
24114 mxy, sy, mzy,
24115 mxz, myz, sz});
24116 assertEquals(calibrator.getInitialMa(), ma1);
24117 final Matrix ma2 = new Matrix(3, 3);
24118 calibrator.getInitialMa(ma2);
24119 assertEquals(ma1, ma2);
24120 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24121 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24122 final NEDPosition nedPosition1 = new NEDPosition();
24123 assertTrue(calibrator.getNedPosition(nedPosition1));
24124 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24125 assertNull(calibrator.getMeasurements());
24126 assertTrue(calibrator.isCommonAxisUsed());
24127 assertNull(calibrator.getListener());
24128 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24129 assertFalse(calibrator.isReady());
24130 assertFalse(calibrator.isRunning());
24131 assertNull(calibrator.getEstimatedBiases());
24132 assertFalse(calibrator.getEstimatedBiases(null));
24133 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24134 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24135 assertNull(calibrator.getEstimatedBiasFx());
24136 assertNull(calibrator.getEstimatedBiasFy());
24137 assertNull(calibrator.getEstimatedBiasFz());
24138 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24139 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24140 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24141 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24142 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24143 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24144 assertNull(calibrator.getEstimatedMa());
24145 assertNull(calibrator.getEstimatedSx());
24146 assertNull(calibrator.getEstimatedSy());
24147 assertNull(calibrator.getEstimatedSz());
24148 assertNull(calibrator.getEstimatedMxy());
24149 assertNull(calibrator.getEstimatedMxz());
24150 assertNull(calibrator.getEstimatedMyx());
24151 assertNull(calibrator.getEstimatedMyz());
24152 assertNull(calibrator.getEstimatedMzx());
24153 assertNull(calibrator.getEstimatedMzy());
24154 assertNull(calibrator.getEstimatedCovariance());
24155 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24156 }
24157
24158 @Test
24159 public void testConstructor200() throws WrongSizeException {
24160 final Matrix ba = generateBa();
24161 final double biasX = ba.getElementAtIndex(0);
24162 final double biasY = ba.getElementAtIndex(1);
24163 final double biasZ = ba.getElementAtIndex(2);
24164
24165 final Matrix ma = generateMaCommonAxis();
24166 final double sx = ma.getElementAt(0, 0);
24167 final double sy = ma.getElementAt(1, 1);
24168 final double sz = ma.getElementAt(2, 2);
24169 final double mxy = ma.getElementAt(0, 1);
24170 final double mxz = ma.getElementAt(0, 2);
24171 final double myx = ma.getElementAt(1, 0);
24172 final double myz = ma.getElementAt(1, 2);
24173 final double mzx = ma.getElementAt(2, 0);
24174 final double mzy = ma.getElementAt(2, 1);
24175
24176 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24177 final double latitude = Math.toRadians(
24178 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24179 final double longitude = Math.toRadians(
24180 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24181 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24182 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24183 final NEDVelocity nedVelocity = new NEDVelocity();
24184 final ECEFPosition ecefPosition = new ECEFPosition();
24185 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24186 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24187 ecefPosition, ecefVelocity);
24188
24189 final KnownPositionAccelerometerCalibrator calibrator =
24190 new KnownPositionAccelerometerCalibrator(nedPosition,
24191 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
24192 myx, myz, mzx, mzy, this);
24193
24194
24195 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24196 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24197 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24198 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24199 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24200 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24201 final Acceleration bx2 = new Acceleration(0.0,
24202 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24203 calibrator.getInitialBiasXAsAcceleration(bx2);
24204 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24205 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24206 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24207 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24208 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24209 final Acceleration by2 = new Acceleration(0.0,
24210 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24211 calibrator.getInitialBiasYAsAcceleration(by2);
24212 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24213 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24214 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24215 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24216 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24217 final Acceleration bz2 = new Acceleration(0.0,
24218 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24219 calibrator.getInitialBiasZAsAcceleration(bz2);
24220 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24221 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24222 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24223 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24224 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24225 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
24226 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
24227 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
24228 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
24229 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
24230 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
24231 final double[] bias1 = calibrator.getInitialBias();
24232 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
24233 final double[] bias2 = new double[3];
24234 calibrator.getInitialBias(bias2);
24235 assertArrayEquals(bias1, bias2, 0.0);
24236 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24237 assertEquals(b1, ba);
24238 final Matrix b2 = new Matrix(3, 1);
24239 calibrator.getInitialBiasAsMatrix(b2);
24240 assertEquals(b1, b2);
24241 final Matrix ma1 = new Matrix(3, 3);
24242 ma1.setSubmatrix(0, 0,
24243 2, 2,
24244 new double[]{ sx, myx, mzx,
24245 mxy, sy, mzy,
24246 mxz, myz, sz});
24247 assertEquals(calibrator.getInitialMa(), ma1);
24248 final Matrix ma2 = new Matrix(3, 3);
24249 calibrator.getInitialMa(ma2);
24250 assertEquals(ma1, ma2);
24251 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24252 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24253 final NEDPosition nedPosition1 = new NEDPosition();
24254 assertTrue(calibrator.getNedPosition(nedPosition1));
24255 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24256 assertNull(calibrator.getMeasurements());
24257 assertTrue(calibrator.isCommonAxisUsed());
24258 assertSame(calibrator.getListener(), this);
24259 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24260 assertFalse(calibrator.isReady());
24261 assertFalse(calibrator.isRunning());
24262 assertNull(calibrator.getEstimatedBiases());
24263 assertFalse(calibrator.getEstimatedBiases(null));
24264 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24265 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24266 assertNull(calibrator.getEstimatedBiasFx());
24267 assertNull(calibrator.getEstimatedBiasFy());
24268 assertNull(calibrator.getEstimatedBiasFz());
24269 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24270 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24271 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24272 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24273 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24274 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24275 assertNull(calibrator.getEstimatedMa());
24276 assertNull(calibrator.getEstimatedSx());
24277 assertNull(calibrator.getEstimatedSy());
24278 assertNull(calibrator.getEstimatedSz());
24279 assertNull(calibrator.getEstimatedMxy());
24280 assertNull(calibrator.getEstimatedMxz());
24281 assertNull(calibrator.getEstimatedMyx());
24282 assertNull(calibrator.getEstimatedMyz());
24283 assertNull(calibrator.getEstimatedMzx());
24284 assertNull(calibrator.getEstimatedMzy());
24285 assertNull(calibrator.getEstimatedCovariance());
24286 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24287 }
24288
24289 @Test
24290 public void testConstructor201() throws WrongSizeException {
24291 final Collection<StandardDeviationBodyKinematics> measurements =
24292 Collections.emptyList();
24293
24294 final Matrix ba = generateBa();
24295 final double biasX = ba.getElementAtIndex(0);
24296 final double biasY = ba.getElementAtIndex(1);
24297 final double biasZ = ba.getElementAtIndex(2);
24298
24299 final Matrix ma = generateMaCommonAxis();
24300 final double sx = ma.getElementAt(0, 0);
24301 final double sy = ma.getElementAt(1, 1);
24302 final double sz = ma.getElementAt(2, 2);
24303 final double mxy = ma.getElementAt(0, 1);
24304 final double mxz = ma.getElementAt(0, 2);
24305 final double myx = ma.getElementAt(1, 0);
24306 final double myz = ma.getElementAt(1, 2);
24307 final double mzx = ma.getElementAt(2, 0);
24308 final double mzy = ma.getElementAt(2, 1);
24309
24310 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24311 final double latitude = Math.toRadians(
24312 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24313 final double longitude = Math.toRadians(
24314 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24315 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24316 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24317 final NEDVelocity nedVelocity = new NEDVelocity();
24318 final ECEFPosition ecefPosition = new ECEFPosition();
24319 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24320 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24321 ecefPosition, ecefVelocity);
24322
24323 final KnownPositionAccelerometerCalibrator calibrator =
24324 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
24325 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
24326 myx, myz, mzx, mzy);
24327
24328
24329 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24330 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24331 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24332 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24333 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24334 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24335 final Acceleration bx2 = new Acceleration(0.0,
24336 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24337 calibrator.getInitialBiasXAsAcceleration(bx2);
24338 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24339 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24340 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24341 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24342 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24343 final Acceleration by2 = new Acceleration(0.0,
24344 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24345 calibrator.getInitialBiasYAsAcceleration(by2);
24346 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24347 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24348 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24349 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24350 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24351 final Acceleration bz2 = new Acceleration(0.0,
24352 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24353 calibrator.getInitialBiasZAsAcceleration(bz2);
24354 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24355 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24356 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24357 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24358 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24359 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
24360 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
24361 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
24362 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
24363 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
24364 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
24365 final double[] bias1 = calibrator.getInitialBias();
24366 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
24367 final double[] bias2 = new double[3];
24368 calibrator.getInitialBias(bias2);
24369 assertArrayEquals(bias1, bias2, 0.0);
24370 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24371 assertEquals(b1, ba);
24372 final Matrix b2 = new Matrix(3, 1);
24373 calibrator.getInitialBiasAsMatrix(b2);
24374 assertEquals(b1, b2);
24375 final Matrix ma1 = new Matrix(3, 3);
24376 ma1.setSubmatrix(0, 0,
24377 2, 2,
24378 new double[]{ sx, myx, mzx,
24379 mxy, sy, mzy,
24380 mxz, myz, sz});
24381 assertEquals(calibrator.getInitialMa(), ma1);
24382 final Matrix ma2 = new Matrix(3, 3);
24383 calibrator.getInitialMa(ma2);
24384 assertEquals(ma1, ma2);
24385 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24386 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24387 final NEDPosition nedPosition1 = new NEDPosition();
24388 assertTrue(calibrator.getNedPosition(nedPosition1));
24389 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24390 assertSame(calibrator.getMeasurements(), measurements);
24391 assertTrue(calibrator.isCommonAxisUsed());
24392 assertNull(calibrator.getListener());
24393 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24394 assertFalse(calibrator.isReady());
24395 assertFalse(calibrator.isRunning());
24396 assertNull(calibrator.getEstimatedBiases());
24397 assertFalse(calibrator.getEstimatedBiases(null));
24398 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24399 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24400 assertNull(calibrator.getEstimatedBiasFx());
24401 assertNull(calibrator.getEstimatedBiasFy());
24402 assertNull(calibrator.getEstimatedBiasFz());
24403 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24404 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24405 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24406 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24407 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24408 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24409 assertNull(calibrator.getEstimatedMa());
24410 assertNull(calibrator.getEstimatedSx());
24411 assertNull(calibrator.getEstimatedSy());
24412 assertNull(calibrator.getEstimatedSz());
24413 assertNull(calibrator.getEstimatedMxy());
24414 assertNull(calibrator.getEstimatedMxz());
24415 assertNull(calibrator.getEstimatedMyx());
24416 assertNull(calibrator.getEstimatedMyz());
24417 assertNull(calibrator.getEstimatedMzx());
24418 assertNull(calibrator.getEstimatedMzy());
24419 assertNull(calibrator.getEstimatedCovariance());
24420 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24421 }
24422
24423 @Test
24424 public void testConstructor202() throws WrongSizeException {
24425 final Collection<StandardDeviationBodyKinematics> measurements =
24426 Collections.emptyList();
24427
24428 final Matrix ba = generateBa();
24429 final double biasX = ba.getElementAtIndex(0);
24430 final double biasY = ba.getElementAtIndex(1);
24431 final double biasZ = ba.getElementAtIndex(2);
24432
24433 final Matrix ma = generateMaCommonAxis();
24434 final double sx = ma.getElementAt(0, 0);
24435 final double sy = ma.getElementAt(1, 1);
24436 final double sz = ma.getElementAt(2, 2);
24437 final double mxy = ma.getElementAt(0, 1);
24438 final double mxz = ma.getElementAt(0, 2);
24439 final double myx = ma.getElementAt(1, 0);
24440 final double myz = ma.getElementAt(1, 2);
24441 final double mzx = ma.getElementAt(2, 0);
24442 final double mzy = ma.getElementAt(2, 1);
24443
24444 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24445 final double latitude = Math.toRadians(
24446 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24447 final double longitude = Math.toRadians(
24448 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24449 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24450 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24451 final NEDVelocity nedVelocity = new NEDVelocity();
24452 final ECEFPosition ecefPosition = new ECEFPosition();
24453 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24454 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24455 ecefPosition, ecefVelocity);
24456
24457 final KnownPositionAccelerometerCalibrator calibrator =
24458 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
24459 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
24460 myx, myz, mzx, mzy, this);
24461
24462
24463 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24464 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24465 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24466 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24467 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24468 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24469 final Acceleration bx2 = new Acceleration(0.0,
24470 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24471 calibrator.getInitialBiasXAsAcceleration(bx2);
24472 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24473 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24474 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24475 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24476 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24477 final Acceleration by2 = new Acceleration(0.0,
24478 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24479 calibrator.getInitialBiasYAsAcceleration(by2);
24480 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24481 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24482 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24483 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24484 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24485 final Acceleration bz2 = new Acceleration(0.0,
24486 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24487 calibrator.getInitialBiasZAsAcceleration(bz2);
24488 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24489 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24490 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24491 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24492 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24493 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
24494 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
24495 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
24496 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
24497 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
24498 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
24499 final double[] bias1 = calibrator.getInitialBias();
24500 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
24501 final double[] bias2 = new double[3];
24502 calibrator.getInitialBias(bias2);
24503 assertArrayEquals(bias1, bias2, 0.0);
24504 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24505 assertEquals(b1, ba);
24506 final Matrix b2 = new Matrix(3, 1);
24507 calibrator.getInitialBiasAsMatrix(b2);
24508 assertEquals(b1, b2);
24509 final Matrix ma1 = new Matrix(3, 3);
24510 ma1.setSubmatrix(0, 0,
24511 2, 2,
24512 new double[]{ sx, myx, mzx,
24513 mxy, sy, mzy,
24514 mxz, myz, sz});
24515 assertEquals(calibrator.getInitialMa(), ma1);
24516 final Matrix ma2 = new Matrix(3, 3);
24517 calibrator.getInitialMa(ma2);
24518 assertEquals(ma1, ma2);
24519 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24520 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24521 final NEDPosition nedPosition1 = new NEDPosition();
24522 assertTrue(calibrator.getNedPosition(nedPosition1));
24523 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24524 assertSame(calibrator.getMeasurements(), measurements);
24525 assertTrue(calibrator.isCommonAxisUsed());
24526 assertSame(calibrator.getListener(), this);
24527 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24528 assertFalse(calibrator.isReady());
24529 assertFalse(calibrator.isRunning());
24530 assertNull(calibrator.getEstimatedBiases());
24531 assertFalse(calibrator.getEstimatedBiases(null));
24532 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24533 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24534 assertNull(calibrator.getEstimatedBiasFx());
24535 assertNull(calibrator.getEstimatedBiasFy());
24536 assertNull(calibrator.getEstimatedBiasFz());
24537 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24538 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24539 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24540 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24541 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24542 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24543 assertNull(calibrator.getEstimatedMa());
24544 assertNull(calibrator.getEstimatedSx());
24545 assertNull(calibrator.getEstimatedSy());
24546 assertNull(calibrator.getEstimatedSz());
24547 assertNull(calibrator.getEstimatedMxy());
24548 assertNull(calibrator.getEstimatedMxz());
24549 assertNull(calibrator.getEstimatedMyx());
24550 assertNull(calibrator.getEstimatedMyz());
24551 assertNull(calibrator.getEstimatedMzx());
24552 assertNull(calibrator.getEstimatedMzy());
24553 assertNull(calibrator.getEstimatedCovariance());
24554 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24555 }
24556
24557 @Test
24558 public void testConstructor203() throws WrongSizeException {
24559 final Matrix ba = generateBa();
24560 final double biasX = ba.getElementAtIndex(0);
24561 final double biasY = ba.getElementAtIndex(1);
24562 final double biasZ = ba.getElementAtIndex(2);
24563
24564 final Acceleration bx = new Acceleration(biasX,
24565 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24566 final Acceleration by = new Acceleration(biasY,
24567 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24568 final Acceleration bz = new Acceleration(biasZ,
24569 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24570
24571 final Matrix ma = generateMaCommonAxis();
24572 final double sx = ma.getElementAt(0, 0);
24573 final double sy = ma.getElementAt(1, 1);
24574 final double sz = ma.getElementAt(2, 2);
24575 final double mxy = ma.getElementAt(0, 1);
24576 final double mxz = ma.getElementAt(0, 2);
24577 final double myx = ma.getElementAt(1, 0);
24578 final double myz = ma.getElementAt(1, 2);
24579 final double mzx = ma.getElementAt(2, 0);
24580 final double mzy = ma.getElementAt(2, 1);
24581
24582 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24583 final double latitude = Math.toRadians(
24584 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24585 final double longitude = Math.toRadians(
24586 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24587 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24588 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24589 final NEDVelocity nedVelocity = new NEDVelocity();
24590 final ECEFPosition ecefPosition = new ECEFPosition();
24591 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24592 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24593 ecefPosition, ecefVelocity);
24594
24595 final KnownPositionAccelerometerCalibrator calibrator =
24596 new KnownPositionAccelerometerCalibrator(nedPosition,
24597 bx, by, bz, sx, sy, sz, mxy, mxz,
24598 myx, myz, mzx, mzy);
24599
24600
24601 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24602 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24603 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24604 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24605 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24606 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24607 final Acceleration bx2 = new Acceleration(0.0,
24608 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24609 calibrator.getInitialBiasXAsAcceleration(bx2);
24610 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24611 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24612 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24613 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24614 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24615 final Acceleration by2 = new Acceleration(0.0,
24616 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24617 calibrator.getInitialBiasYAsAcceleration(by2);
24618 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24619 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24620 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24621 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24622 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24623 final Acceleration bz2 = new Acceleration(0.0,
24624 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24625 calibrator.getInitialBiasZAsAcceleration(bz2);
24626 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24627 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24628 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24629 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24630 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24631 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
24632 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
24633 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
24634 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
24635 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
24636 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
24637 final double[] bias1 = calibrator.getInitialBias();
24638 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
24639 final double[] bias2 = new double[3];
24640 calibrator.getInitialBias(bias2);
24641 assertArrayEquals(bias1, bias2, 0.0);
24642 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24643 assertEquals(b1, ba);
24644 final Matrix b2 = new Matrix(3, 1);
24645 calibrator.getInitialBiasAsMatrix(b2);
24646 assertEquals(b1, b2);
24647 final Matrix ma1 = new Matrix(3, 3);
24648 ma1.setSubmatrix(0, 0,
24649 2, 2,
24650 new double[]{ sx, myx, mzx,
24651 mxy, sy, mzy,
24652 mxz, myz, sz});
24653 assertEquals(calibrator.getInitialMa(), ma1);
24654 final Matrix ma2 = new Matrix(3, 3);
24655 calibrator.getInitialMa(ma2);
24656 assertEquals(ma1, ma2);
24657 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24658 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24659 final NEDPosition nedPosition1 = new NEDPosition();
24660 assertTrue(calibrator.getNedPosition(nedPosition1));
24661 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24662 assertNull(calibrator.getMeasurements());
24663 assertFalse(calibrator.isCommonAxisUsed());
24664 assertNull(calibrator.getListener());
24665 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
24666 assertFalse(calibrator.isReady());
24667 assertFalse(calibrator.isRunning());
24668 assertNull(calibrator.getEstimatedBiases());
24669 assertFalse(calibrator.getEstimatedBiases(null));
24670 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24671 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24672 assertNull(calibrator.getEstimatedBiasFx());
24673 assertNull(calibrator.getEstimatedBiasFy());
24674 assertNull(calibrator.getEstimatedBiasFz());
24675 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24676 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24677 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24678 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24679 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24680 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24681 assertNull(calibrator.getEstimatedMa());
24682 assertNull(calibrator.getEstimatedSx());
24683 assertNull(calibrator.getEstimatedSy());
24684 assertNull(calibrator.getEstimatedSz());
24685 assertNull(calibrator.getEstimatedMxy());
24686 assertNull(calibrator.getEstimatedMxz());
24687 assertNull(calibrator.getEstimatedMyx());
24688 assertNull(calibrator.getEstimatedMyz());
24689 assertNull(calibrator.getEstimatedMzx());
24690 assertNull(calibrator.getEstimatedMzy());
24691 assertNull(calibrator.getEstimatedCovariance());
24692 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24693 }
24694
24695 @Test
24696 public void testConstructor204() throws WrongSizeException {
24697 final Matrix ba = generateBa();
24698 final double biasX = ba.getElementAtIndex(0);
24699 final double biasY = ba.getElementAtIndex(1);
24700 final double biasZ = ba.getElementAtIndex(2);
24701
24702 final Acceleration bx = new Acceleration(biasX,
24703 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24704 final Acceleration by = new Acceleration(biasY,
24705 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24706 final Acceleration bz = new Acceleration(biasZ,
24707 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24708
24709 final Matrix ma = generateMaCommonAxis();
24710 final double sx = ma.getElementAt(0, 0);
24711 final double sy = ma.getElementAt(1, 1);
24712 final double sz = ma.getElementAt(2, 2);
24713 final double mxy = ma.getElementAt(0, 1);
24714 final double mxz = ma.getElementAt(0, 2);
24715 final double myx = ma.getElementAt(1, 0);
24716 final double myz = ma.getElementAt(1, 2);
24717 final double mzx = ma.getElementAt(2, 0);
24718 final double mzy = ma.getElementAt(2, 1);
24719
24720 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24721 final double latitude = Math.toRadians(
24722 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24723 final double longitude = Math.toRadians(
24724 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24725 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24726 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24727 final NEDVelocity nedVelocity = new NEDVelocity();
24728 final ECEFPosition ecefPosition = new ECEFPosition();
24729 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24730 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24731 ecefPosition, ecefVelocity);
24732
24733 final KnownPositionAccelerometerCalibrator calibrator =
24734 new KnownPositionAccelerometerCalibrator(nedPosition,
24735 bx, by, bz, sx, sy, sz, mxy, mxz,
24736 myx, myz, mzx, mzy, this);
24737
24738
24739 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24740 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24741 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24742 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24743 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24744 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24745 final Acceleration bx2 = new Acceleration(0.0,
24746 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24747 calibrator.getInitialBiasXAsAcceleration(bx2);
24748 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24749 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24750 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24751 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24752 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24753 final Acceleration by2 = new Acceleration(0.0,
24754 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24755 calibrator.getInitialBiasYAsAcceleration(by2);
24756 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24757 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24758 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24759 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24760 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24761 final Acceleration bz2 = new Acceleration(0.0,
24762 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24763 calibrator.getInitialBiasZAsAcceleration(bz2);
24764 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24765 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24766 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24767 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24768 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24769 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
24770 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
24771 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
24772 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
24773 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
24774 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
24775 final double[] bias1 = calibrator.getInitialBias();
24776 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
24777 final double[] bias2 = new double[3];
24778 calibrator.getInitialBias(bias2);
24779 assertArrayEquals(bias1, bias2, 0.0);
24780 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24781 assertEquals(b1, ba);
24782 final Matrix b2 = new Matrix(3, 1);
24783 calibrator.getInitialBiasAsMatrix(b2);
24784 assertEquals(b1, b2);
24785 final Matrix ma1 = new Matrix(3, 3);
24786 ma1.setSubmatrix(0, 0,
24787 2, 2,
24788 new double[]{ sx, myx, mzx,
24789 mxy, sy, mzy,
24790 mxz, myz, sz});
24791 assertEquals(calibrator.getInitialMa(), ma1);
24792 final Matrix ma2 = new Matrix(3, 3);
24793 calibrator.getInitialMa(ma2);
24794 assertEquals(ma1, ma2);
24795 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24796 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24797 final NEDPosition nedPosition1 = new NEDPosition();
24798 assertTrue(calibrator.getNedPosition(nedPosition1));
24799 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24800 assertNull(calibrator.getMeasurements());
24801 assertFalse(calibrator.isCommonAxisUsed());
24802 assertSame(calibrator.getListener(), this);
24803 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
24804 assertFalse(calibrator.isReady());
24805 assertFalse(calibrator.isRunning());
24806 assertNull(calibrator.getEstimatedBiases());
24807 assertFalse(calibrator.getEstimatedBiases(null));
24808 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24809 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24810 assertNull(calibrator.getEstimatedBiasFx());
24811 assertNull(calibrator.getEstimatedBiasFy());
24812 assertNull(calibrator.getEstimatedBiasFz());
24813 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24814 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24815 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24816 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24817 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24818 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24819 assertNull(calibrator.getEstimatedMa());
24820 assertNull(calibrator.getEstimatedSx());
24821 assertNull(calibrator.getEstimatedSy());
24822 assertNull(calibrator.getEstimatedSz());
24823 assertNull(calibrator.getEstimatedMxy());
24824 assertNull(calibrator.getEstimatedMxz());
24825 assertNull(calibrator.getEstimatedMyx());
24826 assertNull(calibrator.getEstimatedMyz());
24827 assertNull(calibrator.getEstimatedMzx());
24828 assertNull(calibrator.getEstimatedMzy());
24829 assertNull(calibrator.getEstimatedCovariance());
24830 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24831 }
24832
24833 @Test
24834 public void testConstructor205() throws WrongSizeException {
24835 final Collection<StandardDeviationBodyKinematics> measurements =
24836 Collections.emptyList();
24837
24838 final Matrix ba = generateBa();
24839 final double biasX = ba.getElementAtIndex(0);
24840 final double biasY = ba.getElementAtIndex(1);
24841 final double biasZ = ba.getElementAtIndex(2);
24842
24843 final Acceleration bx = new Acceleration(biasX,
24844 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24845 final Acceleration by = new Acceleration(biasY,
24846 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24847 final Acceleration bz = new Acceleration(biasZ,
24848 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24849
24850 final Matrix ma = generateMaCommonAxis();
24851 final double sx = ma.getElementAt(0, 0);
24852 final double sy = ma.getElementAt(1, 1);
24853 final double sz = ma.getElementAt(2, 2);
24854 final double mxy = ma.getElementAt(0, 1);
24855 final double mxz = ma.getElementAt(0, 2);
24856 final double myx = ma.getElementAt(1, 0);
24857 final double myz = ma.getElementAt(1, 2);
24858 final double mzx = ma.getElementAt(2, 0);
24859 final double mzy = ma.getElementAt(2, 1);
24860
24861 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24862 final double latitude = Math.toRadians(
24863 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24864 final double longitude = Math.toRadians(
24865 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24866 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24867 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24868 final NEDVelocity nedVelocity = new NEDVelocity();
24869 final ECEFPosition ecefPosition = new ECEFPosition();
24870 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24871 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24872 ecefPosition, ecefVelocity);
24873
24874 final KnownPositionAccelerometerCalibrator calibrator =
24875 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
24876 bx, by, bz, sx, sy, sz, mxy, mxz,
24877 myx, myz, mzx, mzy);
24878
24879
24880 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24881 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24882 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24883 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24884 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24885 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24886 final Acceleration bx2 = new Acceleration(0.0,
24887 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24888 calibrator.getInitialBiasXAsAcceleration(bx2);
24889 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24890 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24891 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24892 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24893 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24894 final Acceleration by2 = new Acceleration(0.0,
24895 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24896 calibrator.getInitialBiasYAsAcceleration(by2);
24897 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24898 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24899 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24900 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24901 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24902 final Acceleration bz2 = new Acceleration(0.0,
24903 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24904 calibrator.getInitialBiasZAsAcceleration(bz2);
24905 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24906 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24907 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24908 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24909 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24910 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
24911 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
24912 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
24913 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
24914 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
24915 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
24916 final double[] bias1 = calibrator.getInitialBias();
24917 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
24918 final double[] bias2 = new double[3];
24919 calibrator.getInitialBias(bias2);
24920 assertArrayEquals(bias1, bias2, 0.0);
24921 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24922 assertEquals(b1, ba);
24923 final Matrix b2 = new Matrix(3, 1);
24924 calibrator.getInitialBiasAsMatrix(b2);
24925 assertEquals(b1, b2);
24926 final Matrix ma1 = new Matrix(3, 3);
24927 ma1.setSubmatrix(0, 0,
24928 2, 2,
24929 new double[]{ sx, myx, mzx,
24930 mxy, sy, mzy,
24931 mxz, myz, sz});
24932 assertEquals(calibrator.getInitialMa(), ma1);
24933 final Matrix ma2 = new Matrix(3, 3);
24934 calibrator.getInitialMa(ma2);
24935 assertEquals(ma1, ma2);
24936 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24937 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24938 final NEDPosition nedPosition1 = new NEDPosition();
24939 assertTrue(calibrator.getNedPosition(nedPosition1));
24940 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24941 assertSame(calibrator.getMeasurements(), measurements);
24942 assertFalse(calibrator.isCommonAxisUsed());
24943 assertNull(calibrator.getListener());
24944 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
24945 assertFalse(calibrator.isReady());
24946 assertFalse(calibrator.isRunning());
24947 assertNull(calibrator.getEstimatedBiases());
24948 assertFalse(calibrator.getEstimatedBiases(null));
24949 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24950 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24951 assertNull(calibrator.getEstimatedBiasFx());
24952 assertNull(calibrator.getEstimatedBiasFy());
24953 assertNull(calibrator.getEstimatedBiasFz());
24954 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24955 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24956 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24957 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24958 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24959 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24960 assertNull(calibrator.getEstimatedMa());
24961 assertNull(calibrator.getEstimatedSx());
24962 assertNull(calibrator.getEstimatedSy());
24963 assertNull(calibrator.getEstimatedSz());
24964 assertNull(calibrator.getEstimatedMxy());
24965 assertNull(calibrator.getEstimatedMxz());
24966 assertNull(calibrator.getEstimatedMyx());
24967 assertNull(calibrator.getEstimatedMyz());
24968 assertNull(calibrator.getEstimatedMzx());
24969 assertNull(calibrator.getEstimatedMzy());
24970 assertNull(calibrator.getEstimatedCovariance());
24971 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24972 }
24973
24974 @Test
24975 public void testConstructor206() throws WrongSizeException {
24976 final Collection<StandardDeviationBodyKinematics> measurements =
24977 Collections.emptyList();
24978
24979 final Matrix ba = generateBa();
24980 final double biasX = ba.getElementAtIndex(0);
24981 final double biasY = ba.getElementAtIndex(1);
24982 final double biasZ = ba.getElementAtIndex(2);
24983
24984 final Acceleration bx = new Acceleration(biasX,
24985 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24986 final Acceleration by = new Acceleration(biasY,
24987 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24988 final Acceleration bz = new Acceleration(biasZ,
24989 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24990
24991 final Matrix ma = generateMaCommonAxis();
24992 final double sx = ma.getElementAt(0, 0);
24993 final double sy = ma.getElementAt(1, 1);
24994 final double sz = ma.getElementAt(2, 2);
24995 final double mxy = ma.getElementAt(0, 1);
24996 final double mxz = ma.getElementAt(0, 2);
24997 final double myx = ma.getElementAt(1, 0);
24998 final double myz = ma.getElementAt(1, 2);
24999 final double mzx = ma.getElementAt(2, 0);
25000 final double mzy = ma.getElementAt(2, 1);
25001
25002 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25003 final double latitude = Math.toRadians(
25004 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25005 final double longitude = Math.toRadians(
25006 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25007 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25008 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25009 final NEDVelocity nedVelocity = new NEDVelocity();
25010 final ECEFPosition ecefPosition = new ECEFPosition();
25011 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25012 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25013 ecefPosition, ecefVelocity);
25014
25015 final KnownPositionAccelerometerCalibrator calibrator =
25016 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
25017 bx, by, bz, sx, sy, sz, mxy, mxz,
25018 myx, myz, mzx, mzy, this);
25019
25020
25021 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25022 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25023 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25024 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25025 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25026 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25027 final Acceleration bx2 = new Acceleration(0.0,
25028 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25029 calibrator.getInitialBiasXAsAcceleration(bx2);
25030 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25031 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25032 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25033 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25034 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25035 final Acceleration by2 = new Acceleration(0.0,
25036 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25037 calibrator.getInitialBiasYAsAcceleration(by2);
25038 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25039 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25040 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25041 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25042 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25043 final Acceleration bz2 = new Acceleration(0.0,
25044 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25045 calibrator.getInitialBiasZAsAcceleration(bz2);
25046 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25047 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25048 assertEquals(calibrator.getInitialSx(), sx, 0.0);
25049 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25050 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25051 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25052 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25053 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25054 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25055 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25056 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25057 final double[] bias1 = calibrator.getInitialBias();
25058 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
25059 final double[] bias2 = new double[3];
25060 calibrator.getInitialBias(bias2);
25061 assertArrayEquals(bias1, bias2, 0.0);
25062 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25063 assertEquals(b1, ba);
25064 final Matrix b2 = new Matrix(3, 1);
25065 calibrator.getInitialBiasAsMatrix(b2);
25066 assertEquals(b1, b2);
25067 final Matrix ma1 = new Matrix(3, 3);
25068 ma1.setSubmatrix(0, 0,
25069 2, 2,
25070 new double[]{ sx, myx, mzx,
25071 mxy, sy, mzy,
25072 mxz, myz, sz});
25073 assertEquals(calibrator.getInitialMa(), ma1);
25074 final Matrix ma2 = new Matrix(3, 3);
25075 calibrator.getInitialMa(ma2);
25076 assertEquals(ma1, ma2);
25077 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25078 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25079 final NEDPosition nedPosition1 = new NEDPosition();
25080 assertTrue(calibrator.getNedPosition(nedPosition1));
25081 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25082 assertSame(calibrator.getMeasurements(), measurements);
25083 assertFalse(calibrator.isCommonAxisUsed());
25084 assertSame(calibrator.getListener(), this);
25085 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
25086 assertFalse(calibrator.isReady());
25087 assertFalse(calibrator.isRunning());
25088 assertNull(calibrator.getEstimatedBiases());
25089 assertFalse(calibrator.getEstimatedBiases(null));
25090 assertNull(calibrator.getEstimatedBiasesAsMatrix());
25091 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25092 assertNull(calibrator.getEstimatedBiasFx());
25093 assertNull(calibrator.getEstimatedBiasFy());
25094 assertNull(calibrator.getEstimatedBiasFz());
25095 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25096 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25097 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25098 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25099 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25100 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25101 assertNull(calibrator.getEstimatedMa());
25102 assertNull(calibrator.getEstimatedSx());
25103 assertNull(calibrator.getEstimatedSy());
25104 assertNull(calibrator.getEstimatedSz());
25105 assertNull(calibrator.getEstimatedMxy());
25106 assertNull(calibrator.getEstimatedMxz());
25107 assertNull(calibrator.getEstimatedMyx());
25108 assertNull(calibrator.getEstimatedMyz());
25109 assertNull(calibrator.getEstimatedMzx());
25110 assertNull(calibrator.getEstimatedMzy());
25111 assertNull(calibrator.getEstimatedCovariance());
25112 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25113 }
25114
25115 @Test
25116 public void testConstructor207() throws WrongSizeException {
25117 final Matrix ba = generateBa();
25118 final double biasX = ba.getElementAtIndex(0);
25119 final double biasY = ba.getElementAtIndex(1);
25120 final double biasZ = ba.getElementAtIndex(2);
25121
25122 final Acceleration bx = new Acceleration(biasX,
25123 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25124 final Acceleration by = new Acceleration(biasY,
25125 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25126 final Acceleration bz = new Acceleration(biasZ,
25127 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25128
25129 final Matrix ma = generateMaCommonAxis();
25130 final double sx = ma.getElementAt(0, 0);
25131 final double sy = ma.getElementAt(1, 1);
25132 final double sz = ma.getElementAt(2, 2);
25133 final double mxy = ma.getElementAt(0, 1);
25134 final double mxz = ma.getElementAt(0, 2);
25135 final double myx = ma.getElementAt(1, 0);
25136 final double myz = ma.getElementAt(1, 2);
25137 final double mzx = ma.getElementAt(2, 0);
25138 final double mzy = ma.getElementAt(2, 1);
25139
25140 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25141 final double latitude = Math.toRadians(
25142 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25143 final double longitude = Math.toRadians(
25144 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25145 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25146 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25147 final NEDVelocity nedVelocity = new NEDVelocity();
25148 final ECEFPosition ecefPosition = new ECEFPosition();
25149 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25150 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25151 ecefPosition, ecefVelocity);
25152
25153 final KnownPositionAccelerometerCalibrator calibrator =
25154 new KnownPositionAccelerometerCalibrator(nedPosition,
25155 true, bx, by, bz, sx, sy, sz, mxy, mxz,
25156 myx, myz, mzx, mzy);
25157
25158
25159 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25160 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25161 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25162 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25163 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25164 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25165 final Acceleration bx2 = new Acceleration(0.0,
25166 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25167 calibrator.getInitialBiasXAsAcceleration(bx2);
25168 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25169 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25170 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25171 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25172 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25173 final Acceleration by2 = new Acceleration(0.0,
25174 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25175 calibrator.getInitialBiasYAsAcceleration(by2);
25176 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25177 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25178 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25179 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25180 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25181 final Acceleration bz2 = new Acceleration(0.0,
25182 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25183 calibrator.getInitialBiasZAsAcceleration(bz2);
25184 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25185 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25186 assertEquals(calibrator.getInitialSx(), sx, 0.0);
25187 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25188 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25189 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25190 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25191 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25192 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25193 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25194 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25195 final double[] bias1 = calibrator.getInitialBias();
25196 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
25197 final double[] bias2 = new double[3];
25198 calibrator.getInitialBias(bias2);
25199 assertArrayEquals(bias1, bias2, 0.0);
25200 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25201 assertEquals(b1, ba);
25202 final Matrix b2 = new Matrix(3, 1);
25203 calibrator.getInitialBiasAsMatrix(b2);
25204 assertEquals(b1, b2);
25205 final Matrix ma1 = new Matrix(3, 3);
25206 ma1.setSubmatrix(0, 0,
25207 2, 2,
25208 new double[]{ sx, myx, mzx,
25209 mxy, sy, mzy,
25210 mxz, myz, sz});
25211 assertEquals(calibrator.getInitialMa(), ma1);
25212 final Matrix ma2 = new Matrix(3, 3);
25213 calibrator.getInitialMa(ma2);
25214 assertEquals(ma1, ma2);
25215 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25216 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25217 final NEDPosition nedPosition1 = new NEDPosition();
25218 assertTrue(calibrator.getNedPosition(nedPosition1));
25219 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25220 assertNull(calibrator.getMeasurements());
25221 assertTrue(calibrator.isCommonAxisUsed());
25222 assertNull(calibrator.getListener());
25223 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25224 assertFalse(calibrator.isReady());
25225 assertFalse(calibrator.isRunning());
25226 assertNull(calibrator.getEstimatedBiases());
25227 assertFalse(calibrator.getEstimatedBiases(null));
25228 assertNull(calibrator.getEstimatedBiasesAsMatrix());
25229 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25230 assertNull(calibrator.getEstimatedBiasFx());
25231 assertNull(calibrator.getEstimatedBiasFy());
25232 assertNull(calibrator.getEstimatedBiasFz());
25233 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25234 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25235 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25236 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25237 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25238 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25239 assertNull(calibrator.getEstimatedMa());
25240 assertNull(calibrator.getEstimatedSx());
25241 assertNull(calibrator.getEstimatedSy());
25242 assertNull(calibrator.getEstimatedSz());
25243 assertNull(calibrator.getEstimatedMxy());
25244 assertNull(calibrator.getEstimatedMxz());
25245 assertNull(calibrator.getEstimatedMyx());
25246 assertNull(calibrator.getEstimatedMyz());
25247 assertNull(calibrator.getEstimatedMzx());
25248 assertNull(calibrator.getEstimatedMzy());
25249 assertNull(calibrator.getEstimatedCovariance());
25250 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25251 }
25252
25253 @Test
25254 public void testConstructor208() throws WrongSizeException {
25255 final Matrix ba = generateBa();
25256 final double biasX = ba.getElementAtIndex(0);
25257 final double biasY = ba.getElementAtIndex(1);
25258 final double biasZ = ba.getElementAtIndex(2);
25259
25260 final Acceleration bx = new Acceleration(biasX,
25261 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25262 final Acceleration by = new Acceleration(biasY,
25263 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25264 final Acceleration bz = new Acceleration(biasZ,
25265 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25266
25267 final Matrix ma = generateMaCommonAxis();
25268 final double sx = ma.getElementAt(0, 0);
25269 final double sy = ma.getElementAt(1, 1);
25270 final double sz = ma.getElementAt(2, 2);
25271 final double mxy = ma.getElementAt(0, 1);
25272 final double mxz = ma.getElementAt(0, 2);
25273 final double myx = ma.getElementAt(1, 0);
25274 final double myz = ma.getElementAt(1, 2);
25275 final double mzx = ma.getElementAt(2, 0);
25276 final double mzy = ma.getElementAt(2, 1);
25277
25278 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25279 final double latitude = Math.toRadians(
25280 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25281 final double longitude = Math.toRadians(
25282 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25283 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25284 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25285 final NEDVelocity nedVelocity = new NEDVelocity();
25286 final ECEFPosition ecefPosition = new ECEFPosition();
25287 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25288 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25289 ecefPosition, ecefVelocity);
25290
25291 final KnownPositionAccelerometerCalibrator calibrator =
25292 new KnownPositionAccelerometerCalibrator(nedPosition,
25293 true, bx, by, bz, sx, sy, sz, mxy, mxz,
25294 myx, myz, mzx, mzy, this);
25295
25296
25297 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25298 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25299 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25300 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25301 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25302 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25303 final Acceleration bx2 = new Acceleration(0.0,
25304 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25305 calibrator.getInitialBiasXAsAcceleration(bx2);
25306 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25307 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25308 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25309 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25310 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25311 final Acceleration by2 = new Acceleration(0.0,
25312 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25313 calibrator.getInitialBiasYAsAcceleration(by2);
25314 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25315 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25316 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25317 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25318 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25319 final Acceleration bz2 = new Acceleration(0.0,
25320 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25321 calibrator.getInitialBiasZAsAcceleration(bz2);
25322 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25323 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25324 assertEquals(calibrator.getInitialSx(), sx, 0.0);
25325 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25326 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25327 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25328 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25329 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25330 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25331 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25332 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25333 final double[] bias1 = calibrator.getInitialBias();
25334 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
25335 final double[] bias2 = new double[3];
25336 calibrator.getInitialBias(bias2);
25337 assertArrayEquals(bias1, bias2, 0.0);
25338 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25339 assertEquals(b1, ba);
25340 final Matrix b2 = new Matrix(3, 1);
25341 calibrator.getInitialBiasAsMatrix(b2);
25342 assertEquals(b1, b2);
25343 final Matrix ma1 = new Matrix(3, 3);
25344 ma1.setSubmatrix(0, 0,
25345 2, 2,
25346 new double[]{ sx, myx, mzx,
25347 mxy, sy, mzy,
25348 mxz, myz, sz});
25349 assertEquals(calibrator.getInitialMa(), ma1);
25350 final Matrix ma2 = new Matrix(3, 3);
25351 calibrator.getInitialMa(ma2);
25352 assertEquals(ma1, ma2);
25353 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25354 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25355 final NEDPosition nedPosition1 = new NEDPosition();
25356 assertTrue(calibrator.getNedPosition(nedPosition1));
25357 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25358 assertNull(calibrator.getMeasurements());
25359 assertTrue(calibrator.isCommonAxisUsed());
25360 assertSame(calibrator.getListener(), this);
25361 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25362 assertFalse(calibrator.isReady());
25363 assertFalse(calibrator.isRunning());
25364 assertNull(calibrator.getEstimatedBiases());
25365 assertFalse(calibrator.getEstimatedBiases(null));
25366 assertNull(calibrator.getEstimatedBiasesAsMatrix());
25367 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25368 assertNull(calibrator.getEstimatedBiasFx());
25369 assertNull(calibrator.getEstimatedBiasFy());
25370 assertNull(calibrator.getEstimatedBiasFz());
25371 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25372 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25373 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25374 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25375 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25376 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25377 assertNull(calibrator.getEstimatedMa());
25378 assertNull(calibrator.getEstimatedSx());
25379 assertNull(calibrator.getEstimatedSy());
25380 assertNull(calibrator.getEstimatedSz());
25381 assertNull(calibrator.getEstimatedMxy());
25382 assertNull(calibrator.getEstimatedMxz());
25383 assertNull(calibrator.getEstimatedMyx());
25384 assertNull(calibrator.getEstimatedMyz());
25385 assertNull(calibrator.getEstimatedMzx());
25386 assertNull(calibrator.getEstimatedMzy());
25387 assertNull(calibrator.getEstimatedCovariance());
25388 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25389 }
25390
25391 @Test
25392 public void testConstructor209() throws WrongSizeException {
25393 final Collection<StandardDeviationBodyKinematics> measurements =
25394 Collections.emptyList();
25395
25396 final Matrix ba = generateBa();
25397 final double biasX = ba.getElementAtIndex(0);
25398 final double biasY = ba.getElementAtIndex(1);
25399 final double biasZ = ba.getElementAtIndex(2);
25400
25401 final Acceleration bx = new Acceleration(biasX,
25402 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25403 final Acceleration by = new Acceleration(biasY,
25404 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25405 final Acceleration bz = new Acceleration(biasZ,
25406 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25407
25408 final Matrix ma = generateMaCommonAxis();
25409 final double sx = ma.getElementAt(0, 0);
25410 final double sy = ma.getElementAt(1, 1);
25411 final double sz = ma.getElementAt(2, 2);
25412 final double mxy = ma.getElementAt(0, 1);
25413 final double mxz = ma.getElementAt(0, 2);
25414 final double myx = ma.getElementAt(1, 0);
25415 final double myz = ma.getElementAt(1, 2);
25416 final double mzx = ma.getElementAt(2, 0);
25417 final double mzy = ma.getElementAt(2, 1);
25418
25419 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25420 final double latitude = Math.toRadians(
25421 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25422 final double longitude = Math.toRadians(
25423 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25424 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25425 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25426 final NEDVelocity nedVelocity = new NEDVelocity();
25427 final ECEFPosition ecefPosition = new ECEFPosition();
25428 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25429 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25430 ecefPosition, ecefVelocity);
25431
25432 final KnownPositionAccelerometerCalibrator calibrator =
25433 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
25434 true, bx, by, bz, sx, sy, sz, mxy, mxz,
25435 myx, myz, mzx, mzy);
25436
25437
25438 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25439 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25440 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25441 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25442 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25443 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25444 final Acceleration bx2 = new Acceleration(0.0,
25445 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25446 calibrator.getInitialBiasXAsAcceleration(bx2);
25447 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25448 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25449 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25450 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25451 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25452 final Acceleration by2 = new Acceleration(0.0,
25453 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25454 calibrator.getInitialBiasYAsAcceleration(by2);
25455 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25456 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25457 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25458 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25459 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25460 final Acceleration bz2 = new Acceleration(0.0,
25461 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25462 calibrator.getInitialBiasZAsAcceleration(bz2);
25463 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25464 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25465 assertEquals(calibrator.getInitialSx(), sx, 0.0);
25466 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25467 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25468 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25469 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25470 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25471 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25472 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25473 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25474 final double[] bias1 = calibrator.getInitialBias();
25475 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
25476 final double[] bias2 = new double[3];
25477 calibrator.getInitialBias(bias2);
25478 assertArrayEquals(bias1, bias2, 0.0);
25479 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25480 assertEquals(b1, ba);
25481 final Matrix b2 = new Matrix(3, 1);
25482 calibrator.getInitialBiasAsMatrix(b2);
25483 assertEquals(b1, b2);
25484 final Matrix ma1 = new Matrix(3, 3);
25485 ma1.setSubmatrix(0, 0,
25486 2, 2,
25487 new double[]{ sx, myx, mzx,
25488 mxy, sy, mzy,
25489 mxz, myz, sz});
25490 assertEquals(calibrator.getInitialMa(), ma1);
25491 final Matrix ma2 = new Matrix(3, 3);
25492 calibrator.getInitialMa(ma2);
25493 assertEquals(ma1, ma2);
25494 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25495 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25496 final NEDPosition nedPosition1 = new NEDPosition();
25497 assertTrue(calibrator.getNedPosition(nedPosition1));
25498 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25499 assertSame(calibrator.getMeasurements(), measurements);
25500 assertTrue(calibrator.isCommonAxisUsed());
25501 assertNull(calibrator.getListener());
25502 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25503 assertFalse(calibrator.isReady());
25504 assertFalse(calibrator.isRunning());
25505 assertNull(calibrator.getEstimatedBiases());
25506 assertFalse(calibrator.getEstimatedBiases(null));
25507 assertNull(calibrator.getEstimatedBiasesAsMatrix());
25508 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25509 assertNull(calibrator.getEstimatedBiasFx());
25510 assertNull(calibrator.getEstimatedBiasFy());
25511 assertNull(calibrator.getEstimatedBiasFz());
25512 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25513 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25514 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25515 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25516 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25517 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25518 assertNull(calibrator.getEstimatedMa());
25519 assertNull(calibrator.getEstimatedSx());
25520 assertNull(calibrator.getEstimatedSy());
25521 assertNull(calibrator.getEstimatedSz());
25522 assertNull(calibrator.getEstimatedMxy());
25523 assertNull(calibrator.getEstimatedMxz());
25524 assertNull(calibrator.getEstimatedMyx());
25525 assertNull(calibrator.getEstimatedMyz());
25526 assertNull(calibrator.getEstimatedMzx());
25527 assertNull(calibrator.getEstimatedMzy());
25528 assertNull(calibrator.getEstimatedCovariance());
25529 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25530 }
25531
25532 @Test
25533 public void testConstructor210() throws WrongSizeException {
25534 final Collection<StandardDeviationBodyKinematics> measurements =
25535 Collections.emptyList();
25536
25537 final Matrix ba = generateBa();
25538 final double biasX = ba.getElementAtIndex(0);
25539 final double biasY = ba.getElementAtIndex(1);
25540 final double biasZ = ba.getElementAtIndex(2);
25541
25542 final Acceleration bx = new Acceleration(biasX,
25543 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25544 final Acceleration by = new Acceleration(biasY,
25545 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25546 final Acceleration bz = new Acceleration(biasZ,
25547 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25548
25549 final Matrix ma = generateMaCommonAxis();
25550 final double sx = ma.getElementAt(0, 0);
25551 final double sy = ma.getElementAt(1, 1);
25552 final double sz = ma.getElementAt(2, 2);
25553 final double mxy = ma.getElementAt(0, 1);
25554 final double mxz = ma.getElementAt(0, 2);
25555 final double myx = ma.getElementAt(1, 0);
25556 final double myz = ma.getElementAt(1, 2);
25557 final double mzx = ma.getElementAt(2, 0);
25558 final double mzy = ma.getElementAt(2, 1);
25559
25560 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25561 final double latitude = Math.toRadians(
25562 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25563 final double longitude = Math.toRadians(
25564 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25565 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25566 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25567 final NEDVelocity nedVelocity = new NEDVelocity();
25568 final ECEFPosition ecefPosition = new ECEFPosition();
25569 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25570 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25571 ecefPosition, ecefVelocity);
25572
25573 final KnownPositionAccelerometerCalibrator calibrator =
25574 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
25575 true, bx, by, bz, sx, sy, sz, mxy, mxz,
25576 myx, myz, mzx, mzy, this);
25577
25578
25579 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25580 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25581 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25582 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25583 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25584 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25585 final Acceleration bx2 = new Acceleration(0.0,
25586 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25587 calibrator.getInitialBiasXAsAcceleration(bx2);
25588 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25589 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25590 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25591 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25592 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25593 final Acceleration by2 = new Acceleration(0.0,
25594 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25595 calibrator.getInitialBiasYAsAcceleration(by2);
25596 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25597 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25598 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25599 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25600 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25601 final Acceleration bz2 = new Acceleration(0.0,
25602 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25603 calibrator.getInitialBiasZAsAcceleration(bz2);
25604 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25605 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25606 assertEquals(calibrator.getInitialSx(), sx, 0.0);
25607 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25608 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25609 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25610 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25611 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25612 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25613 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25614 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25615 final double[] bias1 = calibrator.getInitialBias();
25616 assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
25617 final double[] bias2 = new double[3];
25618 calibrator.getInitialBias(bias2);
25619 assertArrayEquals(bias1, bias2, 0.0);
25620 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25621 assertEquals(b1, ba);
25622 final Matrix b2 = new Matrix(3, 1);
25623 calibrator.getInitialBiasAsMatrix(b2);
25624 assertEquals(b1, b2);
25625 final Matrix ma1 = new Matrix(3, 3);
25626 ma1.setSubmatrix(0, 0,
25627 2, 2,
25628 new double[]{ sx, myx, mzx,
25629 mxy, sy, mzy,
25630 mxz, myz, sz});
25631 assertEquals(calibrator.getInitialMa(), ma1);
25632 final Matrix ma2 = new Matrix(3, 3);
25633 calibrator.getInitialMa(ma2);
25634 assertEquals(ma1, ma2);
25635 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25636 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25637 final NEDPosition nedPosition1 = new NEDPosition();
25638 assertTrue(calibrator.getNedPosition(nedPosition1));
25639 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25640 assertSame(calibrator.getMeasurements(), measurements);
25641 assertTrue(calibrator.isCommonAxisUsed());
25642 assertSame(calibrator.getListener(), this);
25643 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25644 assertFalse(calibrator.isReady());
25645 assertFalse(calibrator.isRunning());
25646 assertNull(calibrator.getEstimatedBiases());
25647 assertFalse(calibrator.getEstimatedBiases(null));
25648 assertNull(calibrator.getEstimatedBiasesAsMatrix());
25649 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25650 assertNull(calibrator.getEstimatedBiasFx());
25651 assertNull(calibrator.getEstimatedBiasFy());
25652 assertNull(calibrator.getEstimatedBiasFz());
25653 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25654 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25655 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25656 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25657 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25658 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25659 assertNull(calibrator.getEstimatedMa());
25660 assertNull(calibrator.getEstimatedSx());
25661 assertNull(calibrator.getEstimatedSy());
25662 assertNull(calibrator.getEstimatedSz());
25663 assertNull(calibrator.getEstimatedMxy());
25664 assertNull(calibrator.getEstimatedMxz());
25665 assertNull(calibrator.getEstimatedMyx());
25666 assertNull(calibrator.getEstimatedMyz());
25667 assertNull(calibrator.getEstimatedMzx());
25668 assertNull(calibrator.getEstimatedMzy());
25669 assertNull(calibrator.getEstimatedCovariance());
25670 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25671 }
25672
25673 @Test
25674 public void testConstructor211() throws WrongSizeException {
25675 final Matrix ba = generateBa();
25676 final double[] bias = ba.getBuffer();
25677 final double biasX = ba.getElementAtIndex(0);
25678 final double biasY = ba.getElementAtIndex(1);
25679 final double biasZ = ba.getElementAtIndex(2);
25680
25681 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25682 final double latitude = Math.toRadians(
25683 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25684 final double longitude = Math.toRadians(
25685 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25686 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25687 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25688 final NEDVelocity nedVelocity = new NEDVelocity();
25689 final ECEFPosition ecefPosition = new ECEFPosition();
25690 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25691 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25692 ecefPosition, ecefVelocity);
25693
25694 KnownPositionAccelerometerCalibrator calibrator =
25695 new KnownPositionAccelerometerCalibrator(nedPosition, bias);
25696
25697
25698 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25699 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25700 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25701 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25702 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25703 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25704 final Acceleration bx2 = new Acceleration(0.0,
25705 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25706 calibrator.getInitialBiasXAsAcceleration(bx2);
25707 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25708 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25709 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25710 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25711 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25712 final Acceleration by2 = new Acceleration(0.0,
25713 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25714 calibrator.getInitialBiasYAsAcceleration(by2);
25715 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25716 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25717 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25718 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25719 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25720 final Acceleration bz2 = new Acceleration(0.0,
25721 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25722 calibrator.getInitialBiasZAsAcceleration(bz2);
25723 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25724 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25725 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
25726 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
25727 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
25728 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
25729 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
25730 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
25731 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
25732 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
25733 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
25734 final double[] bias1 = calibrator.getInitialBias();
25735 assertArrayEquals(bias1, bias, 0.0);
25736 final double[] bias2 = new double[3];
25737 calibrator.getInitialBias(bias2);
25738 assertArrayEquals(bias1, bias2, 0.0);
25739 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25740 assertEquals(b1, ba);
25741 final Matrix b2 = new Matrix(3, 1);
25742 calibrator.getInitialBiasAsMatrix(b2);
25743 assertEquals(b1, b2);
25744 final Matrix ma1 = calibrator.getInitialMa();
25745 assertEquals(ma1, new Matrix(3, 3));
25746 final Matrix ma2 = new Matrix(3, 3);
25747 calibrator.getInitialMa(ma2);
25748 assertEquals(ma1, ma2);
25749 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25750 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25751 final NEDPosition nedPosition1 = new NEDPosition();
25752 assertTrue(calibrator.getNedPosition(nedPosition1));
25753 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25754 assertNull(calibrator.getMeasurements());
25755 assertFalse(calibrator.isCommonAxisUsed());
25756 assertNull(calibrator.getListener());
25757 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
25758 assertFalse(calibrator.isReady());
25759 assertFalse(calibrator.isRunning());
25760 assertNull(calibrator.getEstimatedBiases());
25761 assertFalse(calibrator.getEstimatedBiases(null));
25762 assertNull(calibrator.getEstimatedBiasesAsMatrix());
25763 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25764 assertNull(calibrator.getEstimatedBiasFx());
25765 assertNull(calibrator.getEstimatedBiasFy());
25766 assertNull(calibrator.getEstimatedBiasFz());
25767 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25768 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25769 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25770 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25771 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25772 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25773 assertNull(calibrator.getEstimatedMa());
25774 assertNull(calibrator.getEstimatedSx());
25775 assertNull(calibrator.getEstimatedSy());
25776 assertNull(calibrator.getEstimatedSz());
25777 assertNull(calibrator.getEstimatedMxy());
25778 assertNull(calibrator.getEstimatedMxz());
25779 assertNull(calibrator.getEstimatedMyx());
25780 assertNull(calibrator.getEstimatedMyz());
25781 assertNull(calibrator.getEstimatedMzx());
25782 assertNull(calibrator.getEstimatedMzy());
25783 assertNull(calibrator.getEstimatedCovariance());
25784 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25785
25786
25787 calibrator = null;
25788 try {
25789 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
25790 new double[1]);
25791 fail("IllegalArgumentException expected but not thrown");
25792 } catch (final IllegalArgumentException ignore) {
25793 }
25794 assertNull(calibrator);
25795 }
25796
25797 @Test
25798 public void testConstructor212() throws WrongSizeException {
25799 final Matrix ba = generateBa();
25800 final double[] bias = ba.getBuffer();
25801 final double biasX = ba.getElementAtIndex(0);
25802 final double biasY = ba.getElementAtIndex(1);
25803 final double biasZ = ba.getElementAtIndex(2);
25804
25805 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25806 final double latitude = Math.toRadians(
25807 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25808 final double longitude = Math.toRadians(
25809 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25810 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25811 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25812 final NEDVelocity nedVelocity = new NEDVelocity();
25813 final ECEFPosition ecefPosition = new ECEFPosition();
25814 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25815 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25816 ecefPosition, ecefVelocity);
25817
25818 KnownPositionAccelerometerCalibrator calibrator =
25819 new KnownPositionAccelerometerCalibrator(nedPosition, bias,
25820 this);
25821
25822
25823 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25824 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25825 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25826 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25827 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25828 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25829 final Acceleration bx2 = new Acceleration(0.0,
25830 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25831 calibrator.getInitialBiasXAsAcceleration(bx2);
25832 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25833 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25834 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25835 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25836 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25837 final Acceleration by2 = new Acceleration(0.0,
25838 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25839 calibrator.getInitialBiasYAsAcceleration(by2);
25840 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25841 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25842 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25843 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25844 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25845 final Acceleration bz2 = new Acceleration(0.0,
25846 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25847 calibrator.getInitialBiasZAsAcceleration(bz2);
25848 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25849 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25850 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
25851 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
25852 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
25853 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
25854 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
25855 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
25856 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
25857 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
25858 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
25859 final double[] bias1 = calibrator.getInitialBias();
25860 assertArrayEquals(bias1, bias, 0.0);
25861 final double[] bias2 = new double[3];
25862 calibrator.getInitialBias(bias2);
25863 assertArrayEquals(bias1, bias2, 0.0);
25864 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25865 assertEquals(b1, ba);
25866 final Matrix b2 = new Matrix(3, 1);
25867 calibrator.getInitialBiasAsMatrix(b2);
25868 assertEquals(b1, b2);
25869 final Matrix ma1 = calibrator.getInitialMa();
25870 assertEquals(ma1, new Matrix(3, 3));
25871 final Matrix ma2 = new Matrix(3, 3);
25872 calibrator.getInitialMa(ma2);
25873 assertEquals(ma1, ma2);
25874 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25875 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25876 final NEDPosition nedPosition1 = new NEDPosition();
25877 assertTrue(calibrator.getNedPosition(nedPosition1));
25878 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25879 assertNull(calibrator.getMeasurements());
25880 assertFalse(calibrator.isCommonAxisUsed());
25881 assertSame(calibrator.getListener(), this);
25882 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
25883 assertFalse(calibrator.isReady());
25884 assertFalse(calibrator.isRunning());
25885 assertNull(calibrator.getEstimatedBiases());
25886 assertFalse(calibrator.getEstimatedBiases(null));
25887 assertNull(calibrator.getEstimatedBiasesAsMatrix());
25888 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25889 assertNull(calibrator.getEstimatedBiasFx());
25890 assertNull(calibrator.getEstimatedBiasFy());
25891 assertNull(calibrator.getEstimatedBiasFz());
25892 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25893 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25894 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25895 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25896 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25897 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25898 assertNull(calibrator.getEstimatedMa());
25899 assertNull(calibrator.getEstimatedSx());
25900 assertNull(calibrator.getEstimatedSy());
25901 assertNull(calibrator.getEstimatedSz());
25902 assertNull(calibrator.getEstimatedMxy());
25903 assertNull(calibrator.getEstimatedMxz());
25904 assertNull(calibrator.getEstimatedMyx());
25905 assertNull(calibrator.getEstimatedMyz());
25906 assertNull(calibrator.getEstimatedMzx());
25907 assertNull(calibrator.getEstimatedMzy());
25908 assertNull(calibrator.getEstimatedCovariance());
25909 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25910
25911
25912 calibrator = null;
25913 try {
25914 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
25915 new double[1], this);
25916 fail("IllegalArgumentException expected but not thrown");
25917 } catch (final IllegalArgumentException ignore) {
25918 }
25919 assertNull(calibrator);
25920 }
25921
25922 @Test
25923 public void testConstructor213() throws WrongSizeException {
25924 final Collection<StandardDeviationBodyKinematics> measurements =
25925 Collections.emptyList();
25926
25927 final Matrix ba = generateBa();
25928 final double[] bias = ba.getBuffer();
25929 final double biasX = ba.getElementAtIndex(0);
25930 final double biasY = ba.getElementAtIndex(1);
25931 final double biasZ = ba.getElementAtIndex(2);
25932
25933 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25934 final double latitude = Math.toRadians(
25935 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25936 final double longitude = Math.toRadians(
25937 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25938 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25939 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25940 final NEDVelocity nedVelocity = new NEDVelocity();
25941 final ECEFPosition ecefPosition = new ECEFPosition();
25942 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25943 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25944 ecefPosition, ecefVelocity);
25945
25946 KnownPositionAccelerometerCalibrator calibrator =
25947 new KnownPositionAccelerometerCalibrator(nedPosition,
25948 measurements, bias);
25949
25950
25951 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25952 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25953 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25954 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25955 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25956 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25957 final Acceleration bx2 = new Acceleration(0.0,
25958 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25959 calibrator.getInitialBiasXAsAcceleration(bx2);
25960 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25961 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25962 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25963 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25964 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25965 final Acceleration by2 = new Acceleration(0.0,
25966 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25967 calibrator.getInitialBiasYAsAcceleration(by2);
25968 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25969 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25970 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25971 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25972 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25973 final Acceleration bz2 = new Acceleration(0.0,
25974 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25975 calibrator.getInitialBiasZAsAcceleration(bz2);
25976 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25977 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25978 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
25979 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
25980 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
25981 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
25982 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
25983 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
25984 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
25985 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
25986 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
25987 final double[] bias1 = calibrator.getInitialBias();
25988 assertArrayEquals(bias1, bias, 0.0);
25989 final double[] bias2 = new double[3];
25990 calibrator.getInitialBias(bias2);
25991 assertArrayEquals(bias1, bias2, 0.0);
25992 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25993 assertEquals(b1, ba);
25994 final Matrix b2 = new Matrix(3, 1);
25995 calibrator.getInitialBiasAsMatrix(b2);
25996 assertEquals(b1, b2);
25997 final Matrix ma1 = calibrator.getInitialMa();
25998 assertEquals(ma1, new Matrix(3, 3));
25999 final Matrix ma2 = new Matrix(3, 3);
26000 calibrator.getInitialMa(ma2);
26001 assertEquals(ma1, ma2);
26002 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26003 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26004 final NEDPosition nedPosition1 = new NEDPosition();
26005 assertTrue(calibrator.getNedPosition(nedPosition1));
26006 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26007 assertSame(calibrator.getMeasurements(), measurements);
26008 assertFalse(calibrator.isCommonAxisUsed());
26009 assertNull(calibrator.getListener());
26010 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
26011 assertFalse(calibrator.isReady());
26012 assertFalse(calibrator.isRunning());
26013 assertNull(calibrator.getEstimatedBiases());
26014 assertFalse(calibrator.getEstimatedBiases(null));
26015 assertNull(calibrator.getEstimatedBiasesAsMatrix());
26016 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26017 assertNull(calibrator.getEstimatedBiasFx());
26018 assertNull(calibrator.getEstimatedBiasFy());
26019 assertNull(calibrator.getEstimatedBiasFz());
26020 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26021 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26022 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26023 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26024 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26025 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26026 assertNull(calibrator.getEstimatedMa());
26027 assertNull(calibrator.getEstimatedSx());
26028 assertNull(calibrator.getEstimatedSy());
26029 assertNull(calibrator.getEstimatedSz());
26030 assertNull(calibrator.getEstimatedMxy());
26031 assertNull(calibrator.getEstimatedMxz());
26032 assertNull(calibrator.getEstimatedMyx());
26033 assertNull(calibrator.getEstimatedMyz());
26034 assertNull(calibrator.getEstimatedMzx());
26035 assertNull(calibrator.getEstimatedMzy());
26036 assertNull(calibrator.getEstimatedCovariance());
26037 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26038
26039
26040 calibrator = null;
26041 try {
26042 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
26043 measurements, new double[1]);
26044 fail("IllegalArgumentException expected but not thrown");
26045 } catch (final IllegalArgumentException ignore) {
26046 }
26047 assertNull(calibrator);
26048 }
26049
26050 @Test
26051 public void testConstructor214() throws WrongSizeException {
26052 final Collection<StandardDeviationBodyKinematics> measurements =
26053 Collections.emptyList();
26054
26055 final Matrix ba = generateBa();
26056 final double[] bias = ba.getBuffer();
26057 final double biasX = ba.getElementAtIndex(0);
26058 final double biasY = ba.getElementAtIndex(1);
26059 final double biasZ = ba.getElementAtIndex(2);
26060
26061 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26062 final double latitude = Math.toRadians(
26063 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26064 final double longitude = Math.toRadians(
26065 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26066 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26067 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26068 final NEDVelocity nedVelocity = new NEDVelocity();
26069 final ECEFPosition ecefPosition = new ECEFPosition();
26070 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26071 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26072 ecefPosition, ecefVelocity);
26073
26074 KnownPositionAccelerometerCalibrator calibrator =
26075 new KnownPositionAccelerometerCalibrator(nedPosition,
26076 measurements, bias, this);
26077
26078
26079 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26080 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26081 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26082 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26083 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26084 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26085 final Acceleration bx2 = new Acceleration(0.0,
26086 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26087 calibrator.getInitialBiasXAsAcceleration(bx2);
26088 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26089 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26090 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26091 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26092 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26093 final Acceleration by2 = new Acceleration(0.0,
26094 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26095 calibrator.getInitialBiasYAsAcceleration(by2);
26096 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26097 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26098 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26099 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26100 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26101 final Acceleration bz2 = new Acceleration(0.0,
26102 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26103 calibrator.getInitialBiasZAsAcceleration(bz2);
26104 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26105 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26106 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
26107 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
26108 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
26109 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
26110 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
26111 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
26112 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
26113 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
26114 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
26115 final double[] bias1 = calibrator.getInitialBias();
26116 assertArrayEquals(bias1, bias, 0.0);
26117 final double[] bias2 = new double[3];
26118 calibrator.getInitialBias(bias2);
26119 assertArrayEquals(bias1, bias2, 0.0);
26120 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26121 assertEquals(b1, ba);
26122 final Matrix b2 = new Matrix(3, 1);
26123 calibrator.getInitialBiasAsMatrix(b2);
26124 assertEquals(b1, b2);
26125 final Matrix ma1 = calibrator.getInitialMa();
26126 assertEquals(ma1, new Matrix(3, 3));
26127 final Matrix ma2 = new Matrix(3, 3);
26128 calibrator.getInitialMa(ma2);
26129 assertEquals(ma1, ma2);
26130 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26131 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26132 final NEDPosition nedPosition1 = new NEDPosition();
26133 assertTrue(calibrator.getNedPosition(nedPosition1));
26134 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26135 assertSame(calibrator.getMeasurements(), measurements);
26136 assertFalse(calibrator.isCommonAxisUsed());
26137 assertSame(calibrator.getListener(), this);
26138 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
26139 assertFalse(calibrator.isReady());
26140 assertFalse(calibrator.isRunning());
26141 assertNull(calibrator.getEstimatedBiases());
26142 assertFalse(calibrator.getEstimatedBiases(null));
26143 assertNull(calibrator.getEstimatedBiasesAsMatrix());
26144 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26145 assertNull(calibrator.getEstimatedBiasFx());
26146 assertNull(calibrator.getEstimatedBiasFy());
26147 assertNull(calibrator.getEstimatedBiasFz());
26148 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26149 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26150 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26151 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26152 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26153 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26154 assertNull(calibrator.getEstimatedMa());
26155 assertNull(calibrator.getEstimatedSx());
26156 assertNull(calibrator.getEstimatedSy());
26157 assertNull(calibrator.getEstimatedSz());
26158 assertNull(calibrator.getEstimatedMxy());
26159 assertNull(calibrator.getEstimatedMxz());
26160 assertNull(calibrator.getEstimatedMyx());
26161 assertNull(calibrator.getEstimatedMyz());
26162 assertNull(calibrator.getEstimatedMzx());
26163 assertNull(calibrator.getEstimatedMzy());
26164 assertNull(calibrator.getEstimatedCovariance());
26165 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26166
26167
26168 calibrator = null;
26169 try {
26170 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
26171 measurements, new double[1], this);
26172 fail("IllegalArgumentException expected but not thrown");
26173 } catch (final IllegalArgumentException ignore) {
26174 }
26175 assertNull(calibrator);
26176 }
26177
26178 @Test
26179 public void testConstructor215() throws WrongSizeException {
26180 final Matrix ba = generateBa();
26181 final double[] bias = ba.getBuffer();
26182 final double biasX = ba.getElementAtIndex(0);
26183 final double biasY = ba.getElementAtIndex(1);
26184 final double biasZ = ba.getElementAtIndex(2);
26185
26186 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26187 final double latitude = Math.toRadians(
26188 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26189 final double longitude = Math.toRadians(
26190 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26191 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26192 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26193 final NEDVelocity nedVelocity = new NEDVelocity();
26194 final ECEFPosition ecefPosition = new ECEFPosition();
26195 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26196 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26197 ecefPosition, ecefVelocity);
26198
26199 KnownPositionAccelerometerCalibrator calibrator =
26200 new KnownPositionAccelerometerCalibrator(nedPosition,
26201 true, bias);
26202
26203
26204 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26205 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26206 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26207 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26208 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26209 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26210 final Acceleration bx2 = new Acceleration(0.0,
26211 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26212 calibrator.getInitialBiasXAsAcceleration(bx2);
26213 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26214 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26215 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26216 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26217 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26218 final Acceleration by2 = new Acceleration(0.0,
26219 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26220 calibrator.getInitialBiasYAsAcceleration(by2);
26221 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26222 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26223 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26224 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26225 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26226 final Acceleration bz2 = new Acceleration(0.0,
26227 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26228 calibrator.getInitialBiasZAsAcceleration(bz2);
26229 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26230 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26231 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
26232 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
26233 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
26234 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
26235 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
26236 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
26237 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
26238 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
26239 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
26240 final double[] bias1 = calibrator.getInitialBias();
26241 assertArrayEquals(bias1, bias, 0.0);
26242 final double[] bias2 = new double[3];
26243 calibrator.getInitialBias(bias2);
26244 assertArrayEquals(bias1, bias2, 0.0);
26245 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26246 assertEquals(b1, ba);
26247 final Matrix b2 = new Matrix(3, 1);
26248 calibrator.getInitialBiasAsMatrix(b2);
26249 assertEquals(b1, b2);
26250 final Matrix ma1 = calibrator.getInitialMa();
26251 assertEquals(ma1, new Matrix(3, 3));
26252 final Matrix ma2 = new Matrix(3, 3);
26253 calibrator.getInitialMa(ma2);
26254 assertEquals(ma1, ma2);
26255 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26256 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26257 final NEDPosition nedPosition1 = new NEDPosition();
26258 assertTrue(calibrator.getNedPosition(nedPosition1));
26259 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26260 assertNull(calibrator.getMeasurements());
26261 assertTrue(calibrator.isCommonAxisUsed());
26262 assertNull(calibrator.getListener());
26263 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26264 assertFalse(calibrator.isReady());
26265 assertFalse(calibrator.isRunning());
26266 assertNull(calibrator.getEstimatedBiases());
26267 assertFalse(calibrator.getEstimatedBiases(null));
26268 assertNull(calibrator.getEstimatedBiasesAsMatrix());
26269 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26270 assertNull(calibrator.getEstimatedBiasFx());
26271 assertNull(calibrator.getEstimatedBiasFy());
26272 assertNull(calibrator.getEstimatedBiasFz());
26273 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26274 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26275 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26276 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26277 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26278 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26279 assertNull(calibrator.getEstimatedMa());
26280 assertNull(calibrator.getEstimatedSx());
26281 assertNull(calibrator.getEstimatedSy());
26282 assertNull(calibrator.getEstimatedSz());
26283 assertNull(calibrator.getEstimatedMxy());
26284 assertNull(calibrator.getEstimatedMxz());
26285 assertNull(calibrator.getEstimatedMyx());
26286 assertNull(calibrator.getEstimatedMyz());
26287 assertNull(calibrator.getEstimatedMzx());
26288 assertNull(calibrator.getEstimatedMzy());
26289 assertNull(calibrator.getEstimatedCovariance());
26290 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26291
26292
26293 calibrator = null;
26294 try {
26295 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
26296 true, new double[1]);
26297 fail("IllegalArgumentException expected but not thrown");
26298 } catch (final IllegalArgumentException ignore) {
26299 }
26300 assertNull(calibrator);
26301 }
26302
26303 @Test
26304 public void testConstructor216() throws WrongSizeException {
26305 final Matrix ba = generateBa();
26306 final double[] bias = ba.getBuffer();
26307 final double biasX = ba.getElementAtIndex(0);
26308 final double biasY = ba.getElementAtIndex(1);
26309 final double biasZ = ba.getElementAtIndex(2);
26310
26311 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26312 final double latitude = Math.toRadians(
26313 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26314 final double longitude = Math.toRadians(
26315 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26316 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26317 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26318 final NEDVelocity nedVelocity = new NEDVelocity();
26319 final ECEFPosition ecefPosition = new ECEFPosition();
26320 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26321 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26322 ecefPosition, ecefVelocity);
26323
26324 KnownPositionAccelerometerCalibrator calibrator =
26325 new KnownPositionAccelerometerCalibrator(nedPosition,
26326 true, bias, this);
26327
26328
26329 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26330 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26331 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26332 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26333 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26334 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26335 final Acceleration bx2 = new Acceleration(0.0,
26336 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26337 calibrator.getInitialBiasXAsAcceleration(bx2);
26338 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26339 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26340 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26341 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26342 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26343 final Acceleration by2 = new Acceleration(0.0,
26344 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26345 calibrator.getInitialBiasYAsAcceleration(by2);
26346 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26347 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26348 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26349 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26350 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26351 final Acceleration bz2 = new Acceleration(0.0,
26352 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26353 calibrator.getInitialBiasZAsAcceleration(bz2);
26354 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26355 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26356 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
26357 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
26358 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
26359 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
26360 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
26361 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
26362 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
26363 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
26364 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
26365 final double[] bias1 = calibrator.getInitialBias();
26366 assertArrayEquals(bias1, bias, 0.0);
26367 final double[] bias2 = new double[3];
26368 calibrator.getInitialBias(bias2);
26369 assertArrayEquals(bias1, bias2, 0.0);
26370 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26371 assertEquals(b1, ba);
26372 final Matrix b2 = new Matrix(3, 1);
26373 calibrator.getInitialBiasAsMatrix(b2);
26374 assertEquals(b1, b2);
26375 final Matrix ma1 = calibrator.getInitialMa();
26376 assertEquals(ma1, new Matrix(3, 3));
26377 final Matrix ma2 = new Matrix(3, 3);
26378 calibrator.getInitialMa(ma2);
26379 assertEquals(ma1, ma2);
26380 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26381 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26382 final NEDPosition nedPosition1 = new NEDPosition();
26383 assertTrue(calibrator.getNedPosition(nedPosition1));
26384 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26385 assertNull(calibrator.getMeasurements());
26386 assertTrue(calibrator.isCommonAxisUsed());
26387 assertSame(calibrator.getListener(), this);
26388 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26389 assertFalse(calibrator.isReady());
26390 assertFalse(calibrator.isRunning());
26391 assertNull(calibrator.getEstimatedBiases());
26392 assertFalse(calibrator.getEstimatedBiases(null));
26393 assertNull(calibrator.getEstimatedBiasesAsMatrix());
26394 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26395 assertNull(calibrator.getEstimatedBiasFx());
26396 assertNull(calibrator.getEstimatedBiasFy());
26397 assertNull(calibrator.getEstimatedBiasFz());
26398 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26399 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26400 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26401 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26402 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26403 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26404 assertNull(calibrator.getEstimatedMa());
26405 assertNull(calibrator.getEstimatedSx());
26406 assertNull(calibrator.getEstimatedSy());
26407 assertNull(calibrator.getEstimatedSz());
26408 assertNull(calibrator.getEstimatedMxy());
26409 assertNull(calibrator.getEstimatedMxz());
26410 assertNull(calibrator.getEstimatedMyx());
26411 assertNull(calibrator.getEstimatedMyz());
26412 assertNull(calibrator.getEstimatedMzx());
26413 assertNull(calibrator.getEstimatedMzy());
26414 assertNull(calibrator.getEstimatedCovariance());
26415 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26416
26417
26418 calibrator = null;
26419 try {
26420 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
26421 true, new double[1], this);
26422 fail("IllegalArgumentException expected but not thrown");
26423 } catch (final IllegalArgumentException ignore) {
26424 }
26425 assertNull(calibrator);
26426 }
26427
26428 @Test
26429 public void testConstructor217() throws WrongSizeException {
26430 final Collection<StandardDeviationBodyKinematics> measurements =
26431 Collections.emptyList();
26432
26433 final Matrix ba = generateBa();
26434 final double[] bias = ba.getBuffer();
26435 final double biasX = ba.getElementAtIndex(0);
26436 final double biasY = ba.getElementAtIndex(1);
26437 final double biasZ = ba.getElementAtIndex(2);
26438
26439 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26440 final double latitude = Math.toRadians(
26441 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26442 final double longitude = Math.toRadians(
26443 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26444 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26445 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26446 final NEDVelocity nedVelocity = new NEDVelocity();
26447 final ECEFPosition ecefPosition = new ECEFPosition();
26448 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26449 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26450 ecefPosition, ecefVelocity);
26451
26452 KnownPositionAccelerometerCalibrator calibrator =
26453 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
26454 true, bias);
26455
26456
26457 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26458 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26459 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26460 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26461 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26462 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26463 final Acceleration bx2 = new Acceleration(0.0,
26464 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26465 calibrator.getInitialBiasXAsAcceleration(bx2);
26466 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26467 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26468 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26469 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26470 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26471 final Acceleration by2 = new Acceleration(0.0,
26472 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26473 calibrator.getInitialBiasYAsAcceleration(by2);
26474 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26475 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26476 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26477 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26478 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26479 final Acceleration bz2 = new Acceleration(0.0,
26480 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26481 calibrator.getInitialBiasZAsAcceleration(bz2);
26482 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26483 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26484 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
26485 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
26486 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
26487 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
26488 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
26489 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
26490 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
26491 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
26492 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
26493 final double[] bias1 = calibrator.getInitialBias();
26494 assertArrayEquals(bias1, bias, 0.0);
26495 final double[] bias2 = new double[3];
26496 calibrator.getInitialBias(bias2);
26497 assertArrayEquals(bias1, bias2, 0.0);
26498 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26499 assertEquals(b1, ba);
26500 final Matrix b2 = new Matrix(3, 1);
26501 calibrator.getInitialBiasAsMatrix(b2);
26502 assertEquals(b1, b2);
26503 final Matrix ma1 = calibrator.getInitialMa();
26504 assertEquals(ma1, new Matrix(3, 3));
26505 final Matrix ma2 = new Matrix(3, 3);
26506 calibrator.getInitialMa(ma2);
26507 assertEquals(ma1, ma2);
26508 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26509 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26510 final NEDPosition nedPosition1 = new NEDPosition();
26511 assertTrue(calibrator.getNedPosition(nedPosition1));
26512 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26513 assertSame(calibrator.getMeasurements(), measurements);
26514 assertTrue(calibrator.isCommonAxisUsed());
26515 assertNull(calibrator.getListener());
26516 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26517 assertFalse(calibrator.isReady());
26518 assertFalse(calibrator.isRunning());
26519 assertNull(calibrator.getEstimatedBiases());
26520 assertFalse(calibrator.getEstimatedBiases(null));
26521 assertNull(calibrator.getEstimatedBiasesAsMatrix());
26522 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26523 assertNull(calibrator.getEstimatedBiasFx());
26524 assertNull(calibrator.getEstimatedBiasFy());
26525 assertNull(calibrator.getEstimatedBiasFz());
26526 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26527 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26528 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26529 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26530 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26531 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26532 assertNull(calibrator.getEstimatedMa());
26533 assertNull(calibrator.getEstimatedSx());
26534 assertNull(calibrator.getEstimatedSy());
26535 assertNull(calibrator.getEstimatedSz());
26536 assertNull(calibrator.getEstimatedMxy());
26537 assertNull(calibrator.getEstimatedMxz());
26538 assertNull(calibrator.getEstimatedMyx());
26539 assertNull(calibrator.getEstimatedMyz());
26540 assertNull(calibrator.getEstimatedMzx());
26541 assertNull(calibrator.getEstimatedMzy());
26542 assertNull(calibrator.getEstimatedCovariance());
26543 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26544
26545
26546 calibrator = null;
26547 try {
26548 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
26549 measurements, true, new double[1]);
26550 fail("IllegalArgumentException expected but not thrown");
26551 } catch (final IllegalArgumentException ignore) {
26552 }
26553 assertNull(calibrator);
26554 }
26555
26556 @Test
26557 public void testConstructor218() throws WrongSizeException {
26558 final Collection<StandardDeviationBodyKinematics> measurements =
26559 Collections.emptyList();
26560
26561 final Matrix ba = generateBa();
26562 final double[] bias = ba.getBuffer();
26563 final double biasX = ba.getElementAtIndex(0);
26564 final double biasY = ba.getElementAtIndex(1);
26565 final double biasZ = ba.getElementAtIndex(2);
26566
26567 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26568 final double latitude = Math.toRadians(
26569 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26570 final double longitude = Math.toRadians(
26571 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26572 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26573 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26574 final NEDVelocity nedVelocity = new NEDVelocity();
26575 final ECEFPosition ecefPosition = new ECEFPosition();
26576 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26577 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26578 ecefPosition, ecefVelocity);
26579
26580 KnownPositionAccelerometerCalibrator calibrator =
26581 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
26582 true, bias, this);
26583
26584
26585 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26586 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26587 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26588 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26589 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26590 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26591 final Acceleration bx2 = new Acceleration(0.0,
26592 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26593 calibrator.getInitialBiasXAsAcceleration(bx2);
26594 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26595 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26596 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26597 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26598 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26599 final Acceleration by2 = new Acceleration(0.0,
26600 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26601 calibrator.getInitialBiasYAsAcceleration(by2);
26602 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26603 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26604 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26605 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26606 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26607 final Acceleration bz2 = new Acceleration(0.0,
26608 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26609 calibrator.getInitialBiasZAsAcceleration(bz2);
26610 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26611 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26612 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
26613 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
26614 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
26615 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
26616 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
26617 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
26618 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
26619 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
26620 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
26621 final double[] bias1 = calibrator.getInitialBias();
26622 assertArrayEquals(bias1, bias, 0.0);
26623 final double[] bias2 = new double[3];
26624 calibrator.getInitialBias(bias2);
26625 assertArrayEquals(bias1, bias2, 0.0);
26626 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26627 assertEquals(b1, ba);
26628 final Matrix b2 = new Matrix(3, 1);
26629 calibrator.getInitialBiasAsMatrix(b2);
26630 assertEquals(b1, b2);
26631 final Matrix ma1 = calibrator.getInitialMa();
26632 assertEquals(ma1, new Matrix(3, 3));
26633 final Matrix ma2 = new Matrix(3, 3);
26634 calibrator.getInitialMa(ma2);
26635 assertEquals(ma1, ma2);
26636 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26637 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26638 final NEDPosition nedPosition1 = new NEDPosition();
26639 assertTrue(calibrator.getNedPosition(nedPosition1));
26640 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26641 assertSame(calibrator.getMeasurements(), measurements);
26642 assertTrue(calibrator.isCommonAxisUsed());
26643 assertSame(calibrator.getListener(), this);
26644 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26645 assertFalse(calibrator.isReady());
26646 assertFalse(calibrator.isRunning());
26647 assertNull(calibrator.getEstimatedBiases());
26648 assertFalse(calibrator.getEstimatedBiases(null));
26649 assertNull(calibrator.getEstimatedBiasesAsMatrix());
26650 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26651 assertNull(calibrator.getEstimatedBiasFx());
26652 assertNull(calibrator.getEstimatedBiasFy());
26653 assertNull(calibrator.getEstimatedBiasFz());
26654 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26655 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26656 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26657 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26658 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26659 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26660 assertNull(calibrator.getEstimatedMa());
26661 assertNull(calibrator.getEstimatedSx());
26662 assertNull(calibrator.getEstimatedSy());
26663 assertNull(calibrator.getEstimatedSz());
26664 assertNull(calibrator.getEstimatedMxy());
26665 assertNull(calibrator.getEstimatedMxz());
26666 assertNull(calibrator.getEstimatedMyx());
26667 assertNull(calibrator.getEstimatedMyz());
26668 assertNull(calibrator.getEstimatedMzx());
26669 assertNull(calibrator.getEstimatedMzy());
26670 assertNull(calibrator.getEstimatedCovariance());
26671 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26672
26673
26674 calibrator = null;
26675 try {
26676 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
26677 measurements, true, new double[1], this);
26678 fail("IllegalArgumentException expected but not thrown");
26679 } catch (final IllegalArgumentException ignore) {
26680 }
26681 assertNull(calibrator);
26682 }
26683
26684 @Test
26685 public void testConstructor219() throws WrongSizeException {
26686 final Matrix ba = generateBa();
26687 final double[] bias = ba.getBuffer();
26688 final double biasX = ba.getElementAtIndex(0);
26689 final double biasY = ba.getElementAtIndex(1);
26690 final double biasZ = ba.getElementAtIndex(2);
26691
26692 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26693 final double latitude = Math.toRadians(
26694 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26695 final double longitude = Math.toRadians(
26696 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26697 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26698 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26699 final NEDVelocity nedVelocity = new NEDVelocity();
26700 final ECEFPosition ecefPosition = new ECEFPosition();
26701 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26702 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26703 ecefPosition, ecefVelocity);
26704
26705 KnownPositionAccelerometerCalibrator calibrator =
26706 new KnownPositionAccelerometerCalibrator(nedPosition, ba);
26707
26708
26709 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26710 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26711 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26712 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26713 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26714 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26715 final Acceleration bx2 = new Acceleration(0.0,
26716 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26717 calibrator.getInitialBiasXAsAcceleration(bx2);
26718 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26719 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26720 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26721 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26722 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26723 final Acceleration by2 = new Acceleration(0.0,
26724 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26725 calibrator.getInitialBiasYAsAcceleration(by2);
26726 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26727 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26728 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26729 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26730 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26731 final Acceleration bz2 = new Acceleration(0.0,
26732 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26733 calibrator.getInitialBiasZAsAcceleration(bz2);
26734 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26735 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26736 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
26737 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
26738 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
26739 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
26740 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
26741 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
26742 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
26743 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
26744 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
26745 final double[] bias1 = calibrator.getInitialBias();
26746 assertArrayEquals(bias1, bias, 0.0);
26747 final double[] bias2 = new double[3];
26748 calibrator.getInitialBias(bias2);
26749 assertArrayEquals(bias1, bias2, 0.0);
26750 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26751 assertEquals(b1, ba);
26752 final Matrix b2 = new Matrix(3, 1);
26753 calibrator.getInitialBiasAsMatrix(b2);
26754 assertEquals(b1, b2);
26755 final Matrix ma1 = calibrator.getInitialMa();
26756 assertEquals(ma1, new Matrix(3, 3));
26757 final Matrix ma2 = new Matrix(3, 3);
26758 calibrator.getInitialMa(ma2);
26759 assertEquals(ma1, ma2);
26760 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26761 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26762 final NEDPosition nedPosition1 = new NEDPosition();
26763 assertTrue(calibrator.getNedPosition(nedPosition1));
26764 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26765 assertNull(calibrator.getMeasurements());
26766 assertFalse(calibrator.isCommonAxisUsed());
26767 assertNull(calibrator.getListener());
26768 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
26769 assertFalse(calibrator.isReady());
26770 assertFalse(calibrator.isRunning());
26771 assertNull(calibrator.getEstimatedBiases());
26772 assertFalse(calibrator.getEstimatedBiases(null));
26773 assertNull(calibrator.getEstimatedBiasesAsMatrix());
26774 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26775 assertNull(calibrator.getEstimatedBiasFx());
26776 assertNull(calibrator.getEstimatedBiasFy());
26777 assertNull(calibrator.getEstimatedBiasFz());
26778 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26779 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26780 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26781 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26782 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26783 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26784 assertNull(calibrator.getEstimatedMa());
26785 assertNull(calibrator.getEstimatedSx());
26786 assertNull(calibrator.getEstimatedSy());
26787 assertNull(calibrator.getEstimatedSz());
26788 assertNull(calibrator.getEstimatedMxy());
26789 assertNull(calibrator.getEstimatedMxz());
26790 assertNull(calibrator.getEstimatedMyx());
26791 assertNull(calibrator.getEstimatedMyz());
26792 assertNull(calibrator.getEstimatedMzx());
26793 assertNull(calibrator.getEstimatedMzy());
26794 assertNull(calibrator.getEstimatedCovariance());
26795 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26796
26797
26798 calibrator = null;
26799 try {
26800 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
26801 new Matrix(1, 1));
26802 fail("IllegalArgumentException expected but not thrown");
26803 } catch (final IllegalArgumentException ignore) {
26804 }
26805 try {
26806 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
26807 new Matrix(1, 3));
26808 fail("IllegalArgumentException expected but not thrown");
26809 } catch (final IllegalArgumentException ignore) {
26810 }
26811 assertNull(calibrator);
26812 }
26813
26814 @Test
26815 public void testConstructor220() throws WrongSizeException {
26816 final Matrix ba = generateBa();
26817 final double[] bias = ba.getBuffer();
26818 final double biasX = ba.getElementAtIndex(0);
26819 final double biasY = ba.getElementAtIndex(1);
26820 final double biasZ = ba.getElementAtIndex(2);
26821
26822 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26823 final double latitude = Math.toRadians(
26824 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26825 final double longitude = Math.toRadians(
26826 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26827 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26828 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26829 final NEDVelocity nedVelocity = new NEDVelocity();
26830 final ECEFPosition ecefPosition = new ECEFPosition();
26831 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26832 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26833 ecefPosition, ecefVelocity);
26834
26835 KnownPositionAccelerometerCalibrator calibrator =
26836 new KnownPositionAccelerometerCalibrator(nedPosition, ba,
26837 this);
26838
26839
26840 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26841 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26842 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26843 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26844 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26845 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26846 final Acceleration bx2 = new Acceleration(0.0,
26847 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26848 calibrator.getInitialBiasXAsAcceleration(bx2);
26849 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26850 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26851 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26852 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26853 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26854 final Acceleration by2 = new Acceleration(0.0,
26855 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26856 calibrator.getInitialBiasYAsAcceleration(by2);
26857 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26858 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26859 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26860 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26861 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26862 final Acceleration bz2 = new Acceleration(0.0,
26863 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26864 calibrator.getInitialBiasZAsAcceleration(bz2);
26865 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26866 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26867 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
26868 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
26869 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
26870 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
26871 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
26872 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
26873 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
26874 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
26875 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
26876 final double[] bias1 = calibrator.getInitialBias();
26877 assertArrayEquals(bias1, bias, 0.0);
26878 final double[] bias2 = new double[3];
26879 calibrator.getInitialBias(bias2);
26880 assertArrayEquals(bias1, bias2, 0.0);
26881 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26882 assertEquals(b1, ba);
26883 final Matrix b2 = new Matrix(3, 1);
26884 calibrator.getInitialBiasAsMatrix(b2);
26885 assertEquals(b1, b2);
26886 final Matrix ma1 = calibrator.getInitialMa();
26887 assertEquals(ma1, new Matrix(3, 3));
26888 final Matrix ma2 = new Matrix(3, 3);
26889 calibrator.getInitialMa(ma2);
26890 assertEquals(ma1, ma2);
26891 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26892 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26893 final NEDPosition nedPosition1 = new NEDPosition();
26894 assertTrue(calibrator.getNedPosition(nedPosition1));
26895 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26896 assertNull(calibrator.getMeasurements());
26897 assertFalse(calibrator.isCommonAxisUsed());
26898 assertSame(calibrator.getListener(), this);
26899 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
26900 assertFalse(calibrator.isReady());
26901 assertFalse(calibrator.isRunning());
26902 assertNull(calibrator.getEstimatedBiases());
26903 assertFalse(calibrator.getEstimatedBiases(null));
26904 assertNull(calibrator.getEstimatedBiasesAsMatrix());
26905 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26906 assertNull(calibrator.getEstimatedBiasFx());
26907 assertNull(calibrator.getEstimatedBiasFy());
26908 assertNull(calibrator.getEstimatedBiasFz());
26909 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26910 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26911 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26912 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26913 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26914 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26915 assertNull(calibrator.getEstimatedMa());
26916 assertNull(calibrator.getEstimatedSx());
26917 assertNull(calibrator.getEstimatedSy());
26918 assertNull(calibrator.getEstimatedSz());
26919 assertNull(calibrator.getEstimatedMxy());
26920 assertNull(calibrator.getEstimatedMxz());
26921 assertNull(calibrator.getEstimatedMyx());
26922 assertNull(calibrator.getEstimatedMyz());
26923 assertNull(calibrator.getEstimatedMzx());
26924 assertNull(calibrator.getEstimatedMzy());
26925 assertNull(calibrator.getEstimatedCovariance());
26926 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26927
26928
26929 calibrator = null;
26930 try {
26931 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
26932 new Matrix(1, 1), this);
26933 fail("IllegalArgumentException expected but not thrown");
26934 } catch (final IllegalArgumentException ignore) {
26935 }
26936 try {
26937 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
26938 new Matrix(1, 3), this);
26939 fail("IllegalArgumentException expected but not thrown");
26940 } catch (final IllegalArgumentException ignore) {
26941 }
26942 assertNull(calibrator);
26943 }
26944
26945 @Test
26946 public void testConstructor221() throws WrongSizeException {
26947 final Collection<StandardDeviationBodyKinematics> measurements =
26948 Collections.emptyList();
26949
26950 final Matrix ba = generateBa();
26951 final double[] bias = ba.getBuffer();
26952 final double biasX = ba.getElementAtIndex(0);
26953 final double biasY = ba.getElementAtIndex(1);
26954 final double biasZ = ba.getElementAtIndex(2);
26955
26956 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26957 final double latitude = Math.toRadians(
26958 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26959 final double longitude = Math.toRadians(
26960 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26961 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26962 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26963 final NEDVelocity nedVelocity = new NEDVelocity();
26964 final ECEFPosition ecefPosition = new ECEFPosition();
26965 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26966 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26967 ecefPosition, ecefVelocity);
26968
26969 KnownPositionAccelerometerCalibrator calibrator =
26970 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
26971 ba);
26972
26973
26974 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26975 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26976 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26977 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26978 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26979 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26980 final Acceleration bx2 = new Acceleration(0.0,
26981 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26982 calibrator.getInitialBiasXAsAcceleration(bx2);
26983 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26984 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26985 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26986 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26987 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26988 final Acceleration by2 = new Acceleration(0.0,
26989 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26990 calibrator.getInitialBiasYAsAcceleration(by2);
26991 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26992 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26993 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26994 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26995 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26996 final Acceleration bz2 = new Acceleration(0.0,
26997 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26998 calibrator.getInitialBiasZAsAcceleration(bz2);
26999 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27000 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27001 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27002 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27003 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27004 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27005 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27006 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27007 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27008 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27009 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27010 final double[] bias1 = calibrator.getInitialBias();
27011 assertArrayEquals(bias1, bias, 0.0);
27012 final double[] bias2 = new double[3];
27013 calibrator.getInitialBias(bias2);
27014 assertArrayEquals(bias1, bias2, 0.0);
27015 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27016 assertEquals(b1, ba);
27017 final Matrix b2 = new Matrix(3, 1);
27018 calibrator.getInitialBiasAsMatrix(b2);
27019 assertEquals(b1, b2);
27020 final Matrix ma1 = calibrator.getInitialMa();
27021 assertEquals(ma1, new Matrix(3, 3));
27022 final Matrix ma2 = new Matrix(3, 3);
27023 calibrator.getInitialMa(ma2);
27024 assertEquals(ma1, ma2);
27025 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27026 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27027 final NEDPosition nedPosition1 = new NEDPosition();
27028 assertTrue(calibrator.getNedPosition(nedPosition1));
27029 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27030 assertSame(calibrator.getMeasurements(), measurements);
27031 assertFalse(calibrator.isCommonAxisUsed());
27032 assertNull(calibrator.getListener());
27033 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
27034 assertFalse(calibrator.isReady());
27035 assertFalse(calibrator.isRunning());
27036 assertNull(calibrator.getEstimatedBiases());
27037 assertFalse(calibrator.getEstimatedBiases(null));
27038 assertNull(calibrator.getEstimatedBiasesAsMatrix());
27039 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27040 assertNull(calibrator.getEstimatedBiasFx());
27041 assertNull(calibrator.getEstimatedBiasFy());
27042 assertNull(calibrator.getEstimatedBiasFz());
27043 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27044 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27045 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27046 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27047 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27048 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27049 assertNull(calibrator.getEstimatedMa());
27050 assertNull(calibrator.getEstimatedSx());
27051 assertNull(calibrator.getEstimatedSy());
27052 assertNull(calibrator.getEstimatedSz());
27053 assertNull(calibrator.getEstimatedMxy());
27054 assertNull(calibrator.getEstimatedMxz());
27055 assertNull(calibrator.getEstimatedMyx());
27056 assertNull(calibrator.getEstimatedMyz());
27057 assertNull(calibrator.getEstimatedMzx());
27058 assertNull(calibrator.getEstimatedMzy());
27059 assertNull(calibrator.getEstimatedCovariance());
27060 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27061
27062
27063 calibrator = null;
27064 try {
27065 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27066 measurements, new Matrix(1, 1));
27067 fail("IllegalArgumentException expected but not thrown");
27068 } catch (final IllegalArgumentException ignore) {
27069 }
27070 try {
27071 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27072 measurements, new Matrix(1, 3));
27073 fail("IllegalArgumentException expected but not thrown");
27074 } catch (final IllegalArgumentException ignore) {
27075 }
27076 assertNull(calibrator);
27077 }
27078
27079 @Test
27080 public void testConstructor222() throws WrongSizeException {
27081 final Collection<StandardDeviationBodyKinematics> measurements =
27082 Collections.emptyList();
27083
27084 final Matrix ba = generateBa();
27085 final double[] bias = ba.getBuffer();
27086 final double biasX = ba.getElementAtIndex(0);
27087 final double biasY = ba.getElementAtIndex(1);
27088 final double biasZ = ba.getElementAtIndex(2);
27089
27090 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27091 final double latitude = Math.toRadians(
27092 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27093 final double longitude = Math.toRadians(
27094 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27095 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27096 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27097 final NEDVelocity nedVelocity = new NEDVelocity();
27098 final ECEFPosition ecefPosition = new ECEFPosition();
27099 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27100 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27101 ecefPosition, ecefVelocity);
27102
27103 KnownPositionAccelerometerCalibrator calibrator =
27104 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
27105 ba, this);
27106
27107
27108 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27109 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27110 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27111 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27112 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27113 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27114 final Acceleration bx2 = new Acceleration(0.0,
27115 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27116 calibrator.getInitialBiasXAsAcceleration(bx2);
27117 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27118 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27119 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27120 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27121 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27122 final Acceleration by2 = new Acceleration(0.0,
27123 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27124 calibrator.getInitialBiasYAsAcceleration(by2);
27125 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27126 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27127 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27128 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27129 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27130 final Acceleration bz2 = new Acceleration(0.0,
27131 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27132 calibrator.getInitialBiasZAsAcceleration(bz2);
27133 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27134 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27135 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27136 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27137 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27138 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27139 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27140 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27141 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27142 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27143 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27144 final double[] bias1 = calibrator.getInitialBias();
27145 assertArrayEquals(bias1, bias, 0.0);
27146 final double[] bias2 = new double[3];
27147 calibrator.getInitialBias(bias2);
27148 assertArrayEquals(bias1, bias2, 0.0);
27149 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27150 assertEquals(b1, ba);
27151 final Matrix b2 = new Matrix(3, 1);
27152 calibrator.getInitialBiasAsMatrix(b2);
27153 assertEquals(b1, b2);
27154 final Matrix ma1 = calibrator.getInitialMa();
27155 assertEquals(ma1, new Matrix(3, 3));
27156 final Matrix ma2 = new Matrix(3, 3);
27157 calibrator.getInitialMa(ma2);
27158 assertEquals(ma1, ma2);
27159 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27160 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27161 final NEDPosition nedPosition1 = new NEDPosition();
27162 assertTrue(calibrator.getNedPosition(nedPosition1));
27163 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27164 assertSame(calibrator.getMeasurements(), measurements);
27165 assertFalse(calibrator.isCommonAxisUsed());
27166 assertSame(calibrator.getListener(), this);
27167 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
27168 assertFalse(calibrator.isReady());
27169 assertFalse(calibrator.isRunning());
27170 assertNull(calibrator.getEstimatedBiases());
27171 assertFalse(calibrator.getEstimatedBiases(null));
27172 assertNull(calibrator.getEstimatedBiasesAsMatrix());
27173 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27174 assertNull(calibrator.getEstimatedBiasFx());
27175 assertNull(calibrator.getEstimatedBiasFy());
27176 assertNull(calibrator.getEstimatedBiasFz());
27177 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27178 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27179 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27180 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27181 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27182 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27183 assertNull(calibrator.getEstimatedMa());
27184 assertNull(calibrator.getEstimatedSx());
27185 assertNull(calibrator.getEstimatedSy());
27186 assertNull(calibrator.getEstimatedSz());
27187 assertNull(calibrator.getEstimatedMxy());
27188 assertNull(calibrator.getEstimatedMxz());
27189 assertNull(calibrator.getEstimatedMyx());
27190 assertNull(calibrator.getEstimatedMyz());
27191 assertNull(calibrator.getEstimatedMzx());
27192 assertNull(calibrator.getEstimatedMzy());
27193 assertNull(calibrator.getEstimatedCovariance());
27194 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27195
27196
27197 calibrator = null;
27198 try {
27199 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27200 measurements, new Matrix(1, 1), this);
27201 fail("IllegalArgumentException expected but not thrown");
27202 } catch (final IllegalArgumentException ignore) {
27203 }
27204 try {
27205 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27206 measurements, new Matrix(1, 3), this);
27207 fail("IllegalArgumentException expected but not thrown");
27208 } catch (final IllegalArgumentException ignore) {
27209 }
27210 assertNull(calibrator);
27211 }
27212
27213 @Test
27214 public void testConstructor223() throws WrongSizeException {
27215 final Matrix ba = generateBa();
27216 final double[] bias = ba.getBuffer();
27217 final double biasX = ba.getElementAtIndex(0);
27218 final double biasY = ba.getElementAtIndex(1);
27219 final double biasZ = ba.getElementAtIndex(2);
27220
27221 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27222 final double latitude = Math.toRadians(
27223 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27224 final double longitude = Math.toRadians(
27225 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27226 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27227 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27228 final NEDVelocity nedVelocity = new NEDVelocity();
27229 final ECEFPosition ecefPosition = new ECEFPosition();
27230 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27231 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27232 ecefPosition, ecefVelocity);
27233
27234 KnownPositionAccelerometerCalibrator calibrator =
27235 new KnownPositionAccelerometerCalibrator(nedPosition,
27236 true, ba);
27237
27238
27239 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27240 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27241 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27242 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27243 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27244 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27245 final Acceleration bx2 = new Acceleration(0.0,
27246 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27247 calibrator.getInitialBiasXAsAcceleration(bx2);
27248 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27249 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27250 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27251 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27252 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27253 final Acceleration by2 = new Acceleration(0.0,
27254 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27255 calibrator.getInitialBiasYAsAcceleration(by2);
27256 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27257 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27258 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27259 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27260 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27261 final Acceleration bz2 = new Acceleration(0.0,
27262 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27263 calibrator.getInitialBiasZAsAcceleration(bz2);
27264 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27265 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27266 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27267 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27268 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27269 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27270 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27271 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27272 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27273 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27274 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27275 final double[] bias1 = calibrator.getInitialBias();
27276 assertArrayEquals(bias1, bias, 0.0);
27277 final double[] bias2 = new double[3];
27278 calibrator.getInitialBias(bias2);
27279 assertArrayEquals(bias1, bias2, 0.0);
27280 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27281 assertEquals(b1, ba);
27282 final Matrix b2 = new Matrix(3, 1);
27283 calibrator.getInitialBiasAsMatrix(b2);
27284 assertEquals(b1, b2);
27285 final Matrix ma1 = calibrator.getInitialMa();
27286 assertEquals(ma1, new Matrix(3, 3));
27287 final Matrix ma2 = new Matrix(3, 3);
27288 calibrator.getInitialMa(ma2);
27289 assertEquals(ma1, ma2);
27290 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27291 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27292 final NEDPosition nedPosition1 = new NEDPosition();
27293 assertTrue(calibrator.getNedPosition(nedPosition1));
27294 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27295 assertNull(calibrator.getMeasurements());
27296 assertTrue(calibrator.isCommonAxisUsed());
27297 assertNull(calibrator.getListener());
27298 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27299 assertFalse(calibrator.isReady());
27300 assertFalse(calibrator.isRunning());
27301 assertNull(calibrator.getEstimatedBiases());
27302 assertFalse(calibrator.getEstimatedBiases(null));
27303 assertNull(calibrator.getEstimatedBiasesAsMatrix());
27304 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27305 assertNull(calibrator.getEstimatedBiasFx());
27306 assertNull(calibrator.getEstimatedBiasFy());
27307 assertNull(calibrator.getEstimatedBiasFz());
27308 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27309 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27310 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27311 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27312 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27313 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27314 assertNull(calibrator.getEstimatedMa());
27315 assertNull(calibrator.getEstimatedSx());
27316 assertNull(calibrator.getEstimatedSy());
27317 assertNull(calibrator.getEstimatedSz());
27318 assertNull(calibrator.getEstimatedMxy());
27319 assertNull(calibrator.getEstimatedMxz());
27320 assertNull(calibrator.getEstimatedMyx());
27321 assertNull(calibrator.getEstimatedMyz());
27322 assertNull(calibrator.getEstimatedMzx());
27323 assertNull(calibrator.getEstimatedMzy());
27324 assertNull(calibrator.getEstimatedCovariance());
27325 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27326
27327
27328 calibrator = null;
27329 try {
27330 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27331 true, new Matrix(1, 1));
27332 fail("IllegalArgumentException expected but not thrown");
27333 } catch (final IllegalArgumentException ignore) {
27334 }
27335 try {
27336 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27337 true, new Matrix(1, 3));
27338 fail("IllegalArgumentException expected but not thrown");
27339 } catch (final IllegalArgumentException ignore) {
27340 }
27341 assertNull(calibrator);
27342 }
27343
27344 @Test
27345 public void testConstructor224() throws WrongSizeException {
27346 final Matrix ba = generateBa();
27347 final double[] bias = ba.getBuffer();
27348 final double biasX = ba.getElementAtIndex(0);
27349 final double biasY = ba.getElementAtIndex(1);
27350 final double biasZ = ba.getElementAtIndex(2);
27351
27352 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27353 final double latitude = Math.toRadians(
27354 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27355 final double longitude = Math.toRadians(
27356 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27357 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27358 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27359 final NEDVelocity nedVelocity = new NEDVelocity();
27360 final ECEFPosition ecefPosition = new ECEFPosition();
27361 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27362 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27363 ecefPosition, ecefVelocity);
27364
27365 KnownPositionAccelerometerCalibrator calibrator =
27366 new KnownPositionAccelerometerCalibrator(nedPosition,
27367 true, ba, this);
27368
27369
27370 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27371 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27372 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27373 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27374 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27375 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27376 final Acceleration bx2 = new Acceleration(0.0,
27377 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27378 calibrator.getInitialBiasXAsAcceleration(bx2);
27379 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27380 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27381 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27382 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27383 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27384 final Acceleration by2 = new Acceleration(0.0,
27385 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27386 calibrator.getInitialBiasYAsAcceleration(by2);
27387 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27388 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27389 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27390 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27391 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27392 final Acceleration bz2 = new Acceleration(0.0,
27393 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27394 calibrator.getInitialBiasZAsAcceleration(bz2);
27395 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27396 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27397 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27398 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27399 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27400 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27401 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27402 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27403 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27404 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27405 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27406 final double[] bias1 = calibrator.getInitialBias();
27407 assertArrayEquals(bias1, bias, 0.0);
27408 final double[] bias2 = new double[3];
27409 calibrator.getInitialBias(bias2);
27410 assertArrayEquals(bias1, bias2, 0.0);
27411 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27412 assertEquals(b1, ba);
27413 final Matrix b2 = new Matrix(3, 1);
27414 calibrator.getInitialBiasAsMatrix(b2);
27415 assertEquals(b1, b2);
27416 final Matrix ma1 = calibrator.getInitialMa();
27417 assertEquals(ma1, new Matrix(3, 3));
27418 final Matrix ma2 = new Matrix(3, 3);
27419 calibrator.getInitialMa(ma2);
27420 assertEquals(ma1, ma2);
27421 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27422 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27423 final NEDPosition nedPosition1 = new NEDPosition();
27424 assertTrue(calibrator.getNedPosition(nedPosition1));
27425 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27426 assertNull(calibrator.getMeasurements());
27427 assertTrue(calibrator.isCommonAxisUsed());
27428 assertSame(calibrator.getListener(), this);
27429 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27430 assertFalse(calibrator.isReady());
27431 assertFalse(calibrator.isRunning());
27432 assertNull(calibrator.getEstimatedBiases());
27433 assertFalse(calibrator.getEstimatedBiases(null));
27434 assertNull(calibrator.getEstimatedBiasesAsMatrix());
27435 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27436 assertNull(calibrator.getEstimatedBiasFx());
27437 assertNull(calibrator.getEstimatedBiasFy());
27438 assertNull(calibrator.getEstimatedBiasFz());
27439 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27440 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27441 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27442 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27443 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27444 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27445 assertNull(calibrator.getEstimatedMa());
27446 assertNull(calibrator.getEstimatedSx());
27447 assertNull(calibrator.getEstimatedSy());
27448 assertNull(calibrator.getEstimatedSz());
27449 assertNull(calibrator.getEstimatedMxy());
27450 assertNull(calibrator.getEstimatedMxz());
27451 assertNull(calibrator.getEstimatedMyx());
27452 assertNull(calibrator.getEstimatedMyz());
27453 assertNull(calibrator.getEstimatedMzx());
27454 assertNull(calibrator.getEstimatedMzy());
27455 assertNull(calibrator.getEstimatedCovariance());
27456 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27457
27458
27459 calibrator = null;
27460 try {
27461 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27462 true, new Matrix(1, 1),
27463 this);
27464 fail("IllegalArgumentException expected but not thrown");
27465 } catch (final IllegalArgumentException ignore) {
27466 }
27467 try {
27468 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27469 true, new Matrix(1, 3),
27470 this);
27471 fail("IllegalArgumentException expected but not thrown");
27472 } catch (final IllegalArgumentException ignore) {
27473 }
27474 assertNull(calibrator);
27475 }
27476
27477 @Test
27478 public void testConstructor225() throws WrongSizeException {
27479 final Collection<StandardDeviationBodyKinematics> measurements =
27480 Collections.emptyList();
27481
27482 final Matrix ba = generateBa();
27483 final double[] bias = ba.getBuffer();
27484 final double biasX = ba.getElementAtIndex(0);
27485 final double biasY = ba.getElementAtIndex(1);
27486 final double biasZ = ba.getElementAtIndex(2);
27487
27488 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27489 final double latitude = Math.toRadians(
27490 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27491 final double longitude = Math.toRadians(
27492 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27493 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27494 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27495 final NEDVelocity nedVelocity = new NEDVelocity();
27496 final ECEFPosition ecefPosition = new ECEFPosition();
27497 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27498 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27499 ecefPosition, ecefVelocity);
27500
27501 KnownPositionAccelerometerCalibrator calibrator =
27502 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
27503 true, ba);
27504
27505
27506 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27507 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27508 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27509 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27510 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27511 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27512 final Acceleration bx2 = new Acceleration(0.0,
27513 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27514 calibrator.getInitialBiasXAsAcceleration(bx2);
27515 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27516 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27517 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27518 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27519 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27520 final Acceleration by2 = new Acceleration(0.0,
27521 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27522 calibrator.getInitialBiasYAsAcceleration(by2);
27523 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27524 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27525 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27526 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27527 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27528 final Acceleration bz2 = new Acceleration(0.0,
27529 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27530 calibrator.getInitialBiasZAsAcceleration(bz2);
27531 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27532 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27533 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27534 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27535 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27536 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27537 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27538 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27539 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27540 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27541 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27542 final double[] bias1 = calibrator.getInitialBias();
27543 assertArrayEquals(bias1, bias, 0.0);
27544 final double[] bias2 = new double[3];
27545 calibrator.getInitialBias(bias2);
27546 assertArrayEquals(bias1, bias2, 0.0);
27547 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27548 assertEquals(b1, ba);
27549 final Matrix b2 = new Matrix(3, 1);
27550 calibrator.getInitialBiasAsMatrix(b2);
27551 assertEquals(b1, b2);
27552 final Matrix ma1 = calibrator.getInitialMa();
27553 assertEquals(ma1, new Matrix(3, 3));
27554 final Matrix ma2 = new Matrix(3, 3);
27555 calibrator.getInitialMa(ma2);
27556 assertEquals(ma1, ma2);
27557 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27558 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27559 final NEDPosition nedPosition1 = new NEDPosition();
27560 assertTrue(calibrator.getNedPosition(nedPosition1));
27561 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27562 assertSame(calibrator.getMeasurements(), measurements);
27563 assertTrue(calibrator.isCommonAxisUsed());
27564 assertNull(calibrator.getListener());
27565 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27566 assertFalse(calibrator.isReady());
27567 assertFalse(calibrator.isRunning());
27568 assertNull(calibrator.getEstimatedBiases());
27569 assertFalse(calibrator.getEstimatedBiases(null));
27570 assertNull(calibrator.getEstimatedBiasesAsMatrix());
27571 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27572 assertNull(calibrator.getEstimatedBiasFx());
27573 assertNull(calibrator.getEstimatedBiasFy());
27574 assertNull(calibrator.getEstimatedBiasFz());
27575 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27576 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27577 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27578 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27579 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27580 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27581 assertNull(calibrator.getEstimatedMa());
27582 assertNull(calibrator.getEstimatedSx());
27583 assertNull(calibrator.getEstimatedSy());
27584 assertNull(calibrator.getEstimatedSz());
27585 assertNull(calibrator.getEstimatedMxy());
27586 assertNull(calibrator.getEstimatedMxz());
27587 assertNull(calibrator.getEstimatedMyx());
27588 assertNull(calibrator.getEstimatedMyz());
27589 assertNull(calibrator.getEstimatedMzx());
27590 assertNull(calibrator.getEstimatedMzy());
27591 assertNull(calibrator.getEstimatedCovariance());
27592 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27593
27594
27595 calibrator = null;
27596 try {
27597 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27598 measurements, true,
27599 new Matrix(1, 1));
27600 fail("IllegalArgumentException expected but not thrown");
27601 } catch (final IllegalArgumentException ignore) {
27602 }
27603 try {
27604 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27605 measurements,true,
27606 new Matrix(1, 3));
27607 fail("IllegalArgumentException expected but not thrown");
27608 } catch (final IllegalArgumentException ignore) {
27609 }
27610 assertNull(calibrator);
27611 }
27612
27613 @Test
27614 public void testConstructor226() throws WrongSizeException {
27615 final Collection<StandardDeviationBodyKinematics> measurements =
27616 Collections.emptyList();
27617
27618 final Matrix ba = generateBa();
27619 final double[] bias = ba.getBuffer();
27620 final double biasX = ba.getElementAtIndex(0);
27621 final double biasY = ba.getElementAtIndex(1);
27622 final double biasZ = ba.getElementAtIndex(2);
27623
27624 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27625 final double latitude = Math.toRadians(
27626 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27627 final double longitude = Math.toRadians(
27628 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27629 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27630 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27631 final NEDVelocity nedVelocity = new NEDVelocity();
27632 final ECEFPosition ecefPosition = new ECEFPosition();
27633 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27634 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27635 ecefPosition, ecefVelocity);
27636
27637 KnownPositionAccelerometerCalibrator calibrator =
27638 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
27639 true, ba, this);
27640
27641
27642 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27643 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27644 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27645 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27646 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27647 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27648 final Acceleration bx2 = new Acceleration(0.0,
27649 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27650 calibrator.getInitialBiasXAsAcceleration(bx2);
27651 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27652 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27653 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27654 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27655 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27656 final Acceleration by2 = new Acceleration(0.0,
27657 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27658 calibrator.getInitialBiasYAsAcceleration(by2);
27659 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27660 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27661 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27662 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27663 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27664 final Acceleration bz2 = new Acceleration(0.0,
27665 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27666 calibrator.getInitialBiasZAsAcceleration(bz2);
27667 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27668 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27669 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27670 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27671 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27672 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27673 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27674 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27675 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27676 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27677 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27678 final double[] bias1 = calibrator.getInitialBias();
27679 assertArrayEquals(bias1, bias, 0.0);
27680 final double[] bias2 = new double[3];
27681 calibrator.getInitialBias(bias2);
27682 assertArrayEquals(bias1, bias2, 0.0);
27683 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27684 assertEquals(b1, ba);
27685 final Matrix b2 = new Matrix(3, 1);
27686 calibrator.getInitialBiasAsMatrix(b2);
27687 assertEquals(b1, b2);
27688 final Matrix ma1 = calibrator.getInitialMa();
27689 assertEquals(ma1, new Matrix(3, 3));
27690 final Matrix ma2 = new Matrix(3, 3);
27691 calibrator.getInitialMa(ma2);
27692 assertEquals(ma1, ma2);
27693 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27694 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27695 final NEDPosition nedPosition1 = new NEDPosition();
27696 assertTrue(calibrator.getNedPosition(nedPosition1));
27697 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27698 assertSame(calibrator.getMeasurements(), measurements);
27699 assertTrue(calibrator.isCommonAxisUsed());
27700 assertSame(calibrator.getListener(), this);
27701 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27702 assertFalse(calibrator.isReady());
27703 assertFalse(calibrator.isRunning());
27704 assertNull(calibrator.getEstimatedBiases());
27705 assertFalse(calibrator.getEstimatedBiases(null));
27706 assertNull(calibrator.getEstimatedBiasesAsMatrix());
27707 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27708 assertNull(calibrator.getEstimatedBiasFx());
27709 assertNull(calibrator.getEstimatedBiasFy());
27710 assertNull(calibrator.getEstimatedBiasFz());
27711 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27712 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27713 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27714 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27715 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27716 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27717 assertNull(calibrator.getEstimatedMa());
27718 assertNull(calibrator.getEstimatedSx());
27719 assertNull(calibrator.getEstimatedSy());
27720 assertNull(calibrator.getEstimatedSz());
27721 assertNull(calibrator.getEstimatedMxy());
27722 assertNull(calibrator.getEstimatedMxz());
27723 assertNull(calibrator.getEstimatedMyx());
27724 assertNull(calibrator.getEstimatedMyz());
27725 assertNull(calibrator.getEstimatedMzx());
27726 assertNull(calibrator.getEstimatedMzy());
27727 assertNull(calibrator.getEstimatedCovariance());
27728 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27729
27730
27731 calibrator = null;
27732 try {
27733 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27734 measurements, true,
27735 new Matrix(1, 1), this);
27736 fail("IllegalArgumentException expected but not thrown");
27737 } catch (final IllegalArgumentException ignore) {
27738 }
27739 try {
27740 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27741 measurements,true,
27742 new Matrix(1, 3), this);
27743 fail("IllegalArgumentException expected but not thrown");
27744 } catch (final IllegalArgumentException ignore) {
27745 }
27746 assertNull(calibrator);
27747 }
27748
27749 @Test
27750 public void testConstructor227() throws WrongSizeException {
27751 final Matrix ba = generateBa();
27752 final double[] bias = ba.getBuffer();
27753 final double biasX = ba.getElementAtIndex(0);
27754 final double biasY = ba.getElementAtIndex(1);
27755 final double biasZ = ba.getElementAtIndex(2);
27756
27757 final Matrix ma = generateMaCommonAxis();
27758 final double sx = ma.getElementAt(0, 0);
27759 final double sy = ma.getElementAt(1, 1);
27760 final double sz = ma.getElementAt(2, 2);
27761 final double mxy = ma.getElementAt(0, 1);
27762 final double mxz = ma.getElementAt(0, 2);
27763 final double myx = ma.getElementAt(1, 0);
27764 final double myz = ma.getElementAt(1, 2);
27765 final double mzx = ma.getElementAt(2, 0);
27766 final double mzy = ma.getElementAt(2, 1);
27767
27768 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27769 final double latitude = Math.toRadians(
27770 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27771 final double longitude = Math.toRadians(
27772 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27773 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27774 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27775 final NEDVelocity nedVelocity = new NEDVelocity();
27776 final ECEFPosition ecefPosition = new ECEFPosition();
27777 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27778 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27779 ecefPosition, ecefVelocity);
27780
27781 KnownPositionAccelerometerCalibrator calibrator =
27782 new KnownPositionAccelerometerCalibrator(nedPosition, ba, ma);
27783
27784
27785 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27786 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27787 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27788 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27789 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27790 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27791 final Acceleration bx2 = new Acceleration(0.0,
27792 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27793 calibrator.getInitialBiasXAsAcceleration(bx2);
27794 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27795 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27796 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27797 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27798 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27799 final Acceleration by2 = new Acceleration(0.0,
27800 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27801 calibrator.getInitialBiasYAsAcceleration(by2);
27802 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27803 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27804 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27805 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27806 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27807 final Acceleration bz2 = new Acceleration(0.0,
27808 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27809 calibrator.getInitialBiasZAsAcceleration(bz2);
27810 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27811 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27812 assertEquals(calibrator.getInitialSx(), sx, 0.0);
27813 assertEquals(calibrator.getInitialSy(), sy, 0.0);
27814 assertEquals(calibrator.getInitialSz(), sz, 0.0);
27815 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27816 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27817 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27818 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27819 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27820 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27821 final double[] bias1 = calibrator.getInitialBias();
27822 assertArrayEquals(bias1, bias, 0.0);
27823 final double[] bias2 = new double[3];
27824 calibrator.getInitialBias(bias2);
27825 assertArrayEquals(bias1, bias2, 0.0);
27826 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27827 assertEquals(b1, ba);
27828 final Matrix b2 = new Matrix(3, 1);
27829 calibrator.getInitialBiasAsMatrix(b2);
27830 assertEquals(b1, b2);
27831 final Matrix ma1 = new Matrix(3, 3);
27832 ma1.setSubmatrix(0, 0,
27833 2, 2,
27834 new double[]{ sx, myx, mzx,
27835 mxy, sy, mzy,
27836 mxz, myz, sz});
27837 assertEquals(calibrator.getInitialMa(), ma1);
27838 final Matrix ma2 = new Matrix(3, 3);
27839 calibrator.getInitialMa(ma2);
27840 assertEquals(ma1, ma2);
27841 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27842 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27843 final NEDPosition nedPosition1 = new NEDPosition();
27844 assertTrue(calibrator.getNedPosition(nedPosition1));
27845 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27846 assertNull(calibrator.getMeasurements());
27847 assertFalse(calibrator.isCommonAxisUsed());
27848 assertNull(calibrator.getListener());
27849 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
27850 assertFalse(calibrator.isReady());
27851 assertFalse(calibrator.isRunning());
27852 assertNull(calibrator.getEstimatedBiases());
27853 assertFalse(calibrator.getEstimatedBiases(null));
27854 assertNull(calibrator.getEstimatedBiasesAsMatrix());
27855 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27856 assertNull(calibrator.getEstimatedBiasFx());
27857 assertNull(calibrator.getEstimatedBiasFy());
27858 assertNull(calibrator.getEstimatedBiasFz());
27859 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27860 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27861 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27862 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27863 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27864 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27865 assertNull(calibrator.getEstimatedMa());
27866 assertNull(calibrator.getEstimatedSx());
27867 assertNull(calibrator.getEstimatedSy());
27868 assertNull(calibrator.getEstimatedSz());
27869 assertNull(calibrator.getEstimatedMxy());
27870 assertNull(calibrator.getEstimatedMxz());
27871 assertNull(calibrator.getEstimatedMyx());
27872 assertNull(calibrator.getEstimatedMyz());
27873 assertNull(calibrator.getEstimatedMzx());
27874 assertNull(calibrator.getEstimatedMzy());
27875 assertNull(calibrator.getEstimatedCovariance());
27876 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27877
27878
27879 calibrator = null;
27880 try {
27881 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27882 new Matrix(1, 1), ma);
27883 fail("IllegalArgumentException expected but not thrown");
27884 } catch (final IllegalArgumentException ignore) {
27885 }
27886 try {
27887 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27888 new Matrix(1, 3), ma);
27889 fail("IllegalArgumentException expected but not thrown");
27890 } catch (final IllegalArgumentException ignore) {
27891 }
27892 try {
27893 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27894 ba, new Matrix(1, 3));
27895 fail("IllegalArgumentException expected but not thrown");
27896 } catch (final IllegalArgumentException ignore) {
27897 }
27898 try {
27899 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27900 ba, new Matrix(3, 1));
27901 fail("IllegalArgumentException expected but not thrown");
27902 } catch (final IllegalArgumentException ignore) {
27903 }
27904 assertNull(calibrator);
27905 }
27906
27907 @Test
27908 public void testConstructor228() throws WrongSizeException {
27909 final Matrix ba = generateBa();
27910 final double[] bias = ba.getBuffer();
27911 final double biasX = ba.getElementAtIndex(0);
27912 final double biasY = ba.getElementAtIndex(1);
27913 final double biasZ = ba.getElementAtIndex(2);
27914
27915 final Matrix ma = generateMaCommonAxis();
27916 final double sx = ma.getElementAt(0, 0);
27917 final double sy = ma.getElementAt(1, 1);
27918 final double sz = ma.getElementAt(2, 2);
27919 final double mxy = ma.getElementAt(0, 1);
27920 final double mxz = ma.getElementAt(0, 2);
27921 final double myx = ma.getElementAt(1, 0);
27922 final double myz = ma.getElementAt(1, 2);
27923 final double mzx = ma.getElementAt(2, 0);
27924 final double mzy = ma.getElementAt(2, 1);
27925
27926 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27927 final double latitude = Math.toRadians(
27928 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27929 final double longitude = Math.toRadians(
27930 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27931 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27932 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27933 final NEDVelocity nedVelocity = new NEDVelocity();
27934 final ECEFPosition ecefPosition = new ECEFPosition();
27935 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27936 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27937 ecefPosition, ecefVelocity);
27938
27939 KnownPositionAccelerometerCalibrator calibrator =
27940 new KnownPositionAccelerometerCalibrator(nedPosition,
27941 ba, ma, this);
27942
27943
27944 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27945 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27946 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27947 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27948 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27949 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27950 final Acceleration bx2 = new Acceleration(0.0,
27951 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27952 calibrator.getInitialBiasXAsAcceleration(bx2);
27953 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27954 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27955 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27956 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27957 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27958 final Acceleration by2 = new Acceleration(0.0,
27959 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27960 calibrator.getInitialBiasYAsAcceleration(by2);
27961 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27962 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27963 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27964 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27965 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27966 final Acceleration bz2 = new Acceleration(0.0,
27967 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27968 calibrator.getInitialBiasZAsAcceleration(bz2);
27969 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27970 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27971 assertEquals(calibrator.getInitialSx(), sx, 0.0);
27972 assertEquals(calibrator.getInitialSy(), sy, 0.0);
27973 assertEquals(calibrator.getInitialSz(), sz, 0.0);
27974 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27975 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27976 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27977 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27978 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27979 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27980 final double[] bias1 = calibrator.getInitialBias();
27981 assertArrayEquals(bias1, bias, 0.0);
27982 final double[] bias2 = new double[3];
27983 calibrator.getInitialBias(bias2);
27984 assertArrayEquals(bias1, bias2, 0.0);
27985 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27986 assertEquals(b1, ba);
27987 final Matrix b2 = new Matrix(3, 1);
27988 calibrator.getInitialBiasAsMatrix(b2);
27989 assertEquals(b1, b2);
27990 final Matrix ma1 = new Matrix(3, 3);
27991 ma1.setSubmatrix(0, 0,
27992 2, 2,
27993 new double[]{ sx, myx, mzx,
27994 mxy, sy, mzy,
27995 mxz, myz, sz});
27996 assertEquals(calibrator.getInitialMa(), ma1);
27997 final Matrix ma2 = new Matrix(3, 3);
27998 calibrator.getInitialMa(ma2);
27999 assertEquals(ma1, ma2);
28000 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28001 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28002 final NEDPosition nedPosition1 = new NEDPosition();
28003 assertTrue(calibrator.getNedPosition(nedPosition1));
28004 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28005 assertNull(calibrator.getMeasurements());
28006 assertFalse(calibrator.isCommonAxisUsed());
28007 assertSame(calibrator.getListener(), this);
28008 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28009 assertFalse(calibrator.isReady());
28010 assertFalse(calibrator.isRunning());
28011 assertNull(calibrator.getEstimatedBiases());
28012 assertFalse(calibrator.getEstimatedBiases(null));
28013 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28014 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28015 assertNull(calibrator.getEstimatedBiasFx());
28016 assertNull(calibrator.getEstimatedBiasFy());
28017 assertNull(calibrator.getEstimatedBiasFz());
28018 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28019 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28020 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28021 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28022 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28023 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28024 assertNull(calibrator.getEstimatedMa());
28025 assertNull(calibrator.getEstimatedSx());
28026 assertNull(calibrator.getEstimatedSy());
28027 assertNull(calibrator.getEstimatedSz());
28028 assertNull(calibrator.getEstimatedMxy());
28029 assertNull(calibrator.getEstimatedMxz());
28030 assertNull(calibrator.getEstimatedMyx());
28031 assertNull(calibrator.getEstimatedMyz());
28032 assertNull(calibrator.getEstimatedMzx());
28033 assertNull(calibrator.getEstimatedMzy());
28034 assertNull(calibrator.getEstimatedCovariance());
28035 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28036
28037
28038 calibrator = null;
28039 try {
28040 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28041 new Matrix(1, 1), ma, this);
28042 fail("IllegalArgumentException expected but not thrown");
28043 } catch (final IllegalArgumentException ignore) {
28044 }
28045 try {
28046 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28047 new Matrix(1, 3), ma, this);
28048 fail("IllegalArgumentException expected but not thrown");
28049 } catch (final IllegalArgumentException ignore) {
28050 }
28051 try {
28052 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28053 ba, new Matrix(1, 3), this);
28054 fail("IllegalArgumentException expected but not thrown");
28055 } catch (final IllegalArgumentException ignore) {
28056 }
28057 try {
28058 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28059 ba, new Matrix(3, 1), this);
28060 fail("IllegalArgumentException expected but not thrown");
28061 } catch (final IllegalArgumentException ignore) {
28062 }
28063 assertNull(calibrator);
28064 }
28065
28066 @Test
28067 public void testConstructor229() throws WrongSizeException {
28068 final Collection<StandardDeviationBodyKinematics> measurements =
28069 Collections.emptyList();
28070
28071 final Matrix ba = generateBa();
28072 final double[] bias = ba.getBuffer();
28073 final double biasX = ba.getElementAtIndex(0);
28074 final double biasY = ba.getElementAtIndex(1);
28075 final double biasZ = ba.getElementAtIndex(2);
28076
28077 final Matrix ma = generateMaCommonAxis();
28078 final double sx = ma.getElementAt(0, 0);
28079 final double sy = ma.getElementAt(1, 1);
28080 final double sz = ma.getElementAt(2, 2);
28081 final double mxy = ma.getElementAt(0, 1);
28082 final double mxz = ma.getElementAt(0, 2);
28083 final double myx = ma.getElementAt(1, 0);
28084 final double myz = ma.getElementAt(1, 2);
28085 final double mzx = ma.getElementAt(2, 0);
28086 final double mzy = ma.getElementAt(2, 1);
28087
28088 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28089 final double latitude = Math.toRadians(
28090 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28091 final double longitude = Math.toRadians(
28092 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28093 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28094 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28095 final NEDVelocity nedVelocity = new NEDVelocity();
28096 final ECEFPosition ecefPosition = new ECEFPosition();
28097 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28098 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28099 ecefPosition, ecefVelocity);
28100
28101 KnownPositionAccelerometerCalibrator calibrator =
28102 new KnownPositionAccelerometerCalibrator(nedPosition,
28103 measurements, ba, ma);
28104
28105
28106 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28107 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28108 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28109 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28110 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28111 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28112 final Acceleration bx2 = new Acceleration(0.0,
28113 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28114 calibrator.getInitialBiasXAsAcceleration(bx2);
28115 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28116 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28117 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28118 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28119 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28120 final Acceleration by2 = new Acceleration(0.0,
28121 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28122 calibrator.getInitialBiasYAsAcceleration(by2);
28123 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28124 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28125 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28126 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28127 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28128 final Acceleration bz2 = new Acceleration(0.0,
28129 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28130 calibrator.getInitialBiasZAsAcceleration(bz2);
28131 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28132 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28133 assertEquals(calibrator.getInitialSx(), sx, 0.0);
28134 assertEquals(calibrator.getInitialSy(), sy, 0.0);
28135 assertEquals(calibrator.getInitialSz(), sz, 0.0);
28136 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
28137 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
28138 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
28139 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
28140 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
28141 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
28142 final double[] bias1 = calibrator.getInitialBias();
28143 assertArrayEquals(bias1, bias, 0.0);
28144 final double[] bias2 = new double[3];
28145 calibrator.getInitialBias(bias2);
28146 assertArrayEquals(bias1, bias2, 0.0);
28147 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28148 assertEquals(b1, ba);
28149 final Matrix b2 = new Matrix(3, 1);
28150 calibrator.getInitialBiasAsMatrix(b2);
28151 assertEquals(b1, b2);
28152 final Matrix ma1 = new Matrix(3, 3);
28153 ma1.setSubmatrix(0, 0,
28154 2, 2,
28155 new double[]{ sx, myx, mzx,
28156 mxy, sy, mzy,
28157 mxz, myz, sz});
28158 assertEquals(calibrator.getInitialMa(), ma1);
28159 final Matrix ma2 = new Matrix(3, 3);
28160 calibrator.getInitialMa(ma2);
28161 assertEquals(ma1, ma2);
28162 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28163 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28164 final NEDPosition nedPosition1 = new NEDPosition();
28165 assertTrue(calibrator.getNedPosition(nedPosition1));
28166 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28167 assertSame(calibrator.getMeasurements(), measurements);
28168 assertFalse(calibrator.isCommonAxisUsed());
28169 assertNull(calibrator.getListener());
28170 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28171 assertFalse(calibrator.isReady());
28172 assertFalse(calibrator.isRunning());
28173 assertNull(calibrator.getEstimatedBiases());
28174 assertFalse(calibrator.getEstimatedBiases(null));
28175 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28176 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28177 assertNull(calibrator.getEstimatedBiasFx());
28178 assertNull(calibrator.getEstimatedBiasFy());
28179 assertNull(calibrator.getEstimatedBiasFz());
28180 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28181 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28182 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28183 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28184 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28185 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28186 assertNull(calibrator.getEstimatedMa());
28187 assertNull(calibrator.getEstimatedSx());
28188 assertNull(calibrator.getEstimatedSy());
28189 assertNull(calibrator.getEstimatedSz());
28190 assertNull(calibrator.getEstimatedMxy());
28191 assertNull(calibrator.getEstimatedMxz());
28192 assertNull(calibrator.getEstimatedMyx());
28193 assertNull(calibrator.getEstimatedMyz());
28194 assertNull(calibrator.getEstimatedMzx());
28195 assertNull(calibrator.getEstimatedMzy());
28196 assertNull(calibrator.getEstimatedCovariance());
28197 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28198
28199
28200 calibrator = null;
28201 try {
28202 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28203 measurements, new Matrix(1, 1), ma);
28204 fail("IllegalArgumentException expected but not thrown");
28205 } catch (final IllegalArgumentException ignore) {
28206 }
28207 try {
28208 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28209 measurements, new Matrix(1, 3), ma);
28210 fail("IllegalArgumentException expected but not thrown");
28211 } catch (final IllegalArgumentException ignore) {
28212 }
28213 try {
28214 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28215 measurements, ba, new Matrix(1, 3));
28216 fail("IllegalArgumentException expected but not thrown");
28217 } catch (final IllegalArgumentException ignore) {
28218 }
28219 try {
28220 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28221 measurements, ba, new Matrix(3, 1));
28222 fail("IllegalArgumentException expected but not thrown");
28223 } catch (final IllegalArgumentException ignore) {
28224 }
28225 assertNull(calibrator);
28226 }
28227
28228 @Test
28229 public void testConstructor230() throws WrongSizeException {
28230 final Collection<StandardDeviationBodyKinematics> measurements =
28231 Collections.emptyList();
28232
28233 final Matrix ba = generateBa();
28234 final double[] bias = ba.getBuffer();
28235 final double biasX = ba.getElementAtIndex(0);
28236 final double biasY = ba.getElementAtIndex(1);
28237 final double biasZ = ba.getElementAtIndex(2);
28238
28239 final Matrix ma = generateMaCommonAxis();
28240 final double sx = ma.getElementAt(0, 0);
28241 final double sy = ma.getElementAt(1, 1);
28242 final double sz = ma.getElementAt(2, 2);
28243 final double mxy = ma.getElementAt(0, 1);
28244 final double mxz = ma.getElementAt(0, 2);
28245 final double myx = ma.getElementAt(1, 0);
28246 final double myz = ma.getElementAt(1, 2);
28247 final double mzx = ma.getElementAt(2, 0);
28248 final double mzy = ma.getElementAt(2, 1);
28249
28250 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28251 final double latitude = Math.toRadians(
28252 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28253 final double longitude = Math.toRadians(
28254 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28255 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28256 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28257 final NEDVelocity nedVelocity = new NEDVelocity();
28258 final ECEFPosition ecefPosition = new ECEFPosition();
28259 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28260 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28261 ecefPosition, ecefVelocity);
28262
28263 KnownPositionAccelerometerCalibrator calibrator =
28264 new KnownPositionAccelerometerCalibrator(nedPosition,
28265 measurements, ba, ma, this);
28266
28267
28268 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28269 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28270 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28271 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28272 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28273 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28274 final Acceleration bx2 = new Acceleration(0.0,
28275 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28276 calibrator.getInitialBiasXAsAcceleration(bx2);
28277 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28278 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28279 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28280 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28281 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28282 final Acceleration by2 = new Acceleration(0.0,
28283 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28284 calibrator.getInitialBiasYAsAcceleration(by2);
28285 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28286 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28287 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28288 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28289 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28290 final Acceleration bz2 = new Acceleration(0.0,
28291 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28292 calibrator.getInitialBiasZAsAcceleration(bz2);
28293 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28294 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28295 assertEquals(calibrator.getInitialSx(), sx, 0.0);
28296 assertEquals(calibrator.getInitialSy(), sy, 0.0);
28297 assertEquals(calibrator.getInitialSz(), sz, 0.0);
28298 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
28299 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
28300 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
28301 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
28302 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
28303 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
28304 final double[] bias1 = calibrator.getInitialBias();
28305 assertArrayEquals(bias1, bias, 0.0);
28306 final double[] bias2 = new double[3];
28307 calibrator.getInitialBias(bias2);
28308 assertArrayEquals(bias1, bias2, 0.0);
28309 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28310 assertEquals(b1, ba);
28311 final Matrix b2 = new Matrix(3, 1);
28312 calibrator.getInitialBiasAsMatrix(b2);
28313 assertEquals(b1, b2);
28314 final Matrix ma1 = new Matrix(3, 3);
28315 ma1.setSubmatrix(0, 0,
28316 2, 2,
28317 new double[]{ sx, myx, mzx,
28318 mxy, sy, mzy,
28319 mxz, myz, sz});
28320 assertEquals(calibrator.getInitialMa(), ma1);
28321 final Matrix ma2 = new Matrix(3, 3);
28322 calibrator.getInitialMa(ma2);
28323 assertEquals(ma1, ma2);
28324 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28325 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28326 final NEDPosition nedPosition1 = new NEDPosition();
28327 assertTrue(calibrator.getNedPosition(nedPosition1));
28328 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28329 assertSame(calibrator.getMeasurements(), measurements);
28330 assertFalse(calibrator.isCommonAxisUsed());
28331 assertSame(calibrator.getListener(), this);
28332 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28333 assertFalse(calibrator.isReady());
28334 assertFalse(calibrator.isRunning());
28335 assertNull(calibrator.getEstimatedBiases());
28336 assertFalse(calibrator.getEstimatedBiases(null));
28337 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28338 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28339 assertNull(calibrator.getEstimatedBiasFx());
28340 assertNull(calibrator.getEstimatedBiasFy());
28341 assertNull(calibrator.getEstimatedBiasFz());
28342 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28343 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28344 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28345 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28346 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28347 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28348 assertNull(calibrator.getEstimatedMa());
28349 assertNull(calibrator.getEstimatedSx());
28350 assertNull(calibrator.getEstimatedSy());
28351 assertNull(calibrator.getEstimatedSz());
28352 assertNull(calibrator.getEstimatedMxy());
28353 assertNull(calibrator.getEstimatedMxz());
28354 assertNull(calibrator.getEstimatedMyx());
28355 assertNull(calibrator.getEstimatedMyz());
28356 assertNull(calibrator.getEstimatedMzx());
28357 assertNull(calibrator.getEstimatedMzy());
28358 assertNull(calibrator.getEstimatedCovariance());
28359 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28360
28361
28362 calibrator = null;
28363 try {
28364 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28365 measurements, new Matrix(1, 1), ma, this);
28366 fail("IllegalArgumentException expected but not thrown");
28367 } catch (final IllegalArgumentException ignore) {
28368 }
28369 try {
28370 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28371 measurements, new Matrix(1, 3), ma, this);
28372 fail("IllegalArgumentException expected but not thrown");
28373 } catch (final IllegalArgumentException ignore) {
28374 }
28375 try {
28376 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28377 measurements, ba, new Matrix(1, 3), this);
28378 fail("IllegalArgumentException expected but not thrown");
28379 } catch (final IllegalArgumentException ignore) {
28380 }
28381 try {
28382 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28383 measurements, ba, new Matrix(3, 1), this);
28384 fail("IllegalArgumentException expected but not thrown");
28385 } catch (final IllegalArgumentException ignore) {
28386 }
28387 assertNull(calibrator);
28388 }
28389
28390 @Test
28391 public void testConstructor231() throws WrongSizeException {
28392 final Matrix ba = generateBa();
28393 final double[] bias = ba.getBuffer();
28394 final double biasX = ba.getElementAtIndex(0);
28395 final double biasY = ba.getElementAtIndex(1);
28396 final double biasZ = ba.getElementAtIndex(2);
28397
28398 final Matrix ma = generateMaCommonAxis();
28399 final double sx = ma.getElementAt(0, 0);
28400 final double sy = ma.getElementAt(1, 1);
28401 final double sz = ma.getElementAt(2, 2);
28402 final double mxy = ma.getElementAt(0, 1);
28403 final double mxz = ma.getElementAt(0, 2);
28404 final double myx = ma.getElementAt(1, 0);
28405 final double myz = ma.getElementAt(1, 2);
28406 final double mzx = ma.getElementAt(2, 0);
28407 final double mzy = ma.getElementAt(2, 1);
28408
28409 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28410 final double latitude = Math.toRadians(
28411 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28412 final double longitude = Math.toRadians(
28413 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28414 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28415 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28416 final NEDVelocity nedVelocity = new NEDVelocity();
28417 final ECEFPosition ecefPosition = new ECEFPosition();
28418 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28419 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28420 ecefPosition, ecefVelocity);
28421
28422 KnownPositionAccelerometerCalibrator calibrator =
28423 new KnownPositionAccelerometerCalibrator(nedPosition,
28424 true, ba, ma);
28425
28426
28427 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28428 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28429 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28430 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28431 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28432 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28433 final Acceleration bx2 = new Acceleration(0.0,
28434 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28435 calibrator.getInitialBiasXAsAcceleration(bx2);
28436 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28437 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28438 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28439 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28440 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28441 final Acceleration by2 = new Acceleration(0.0,
28442 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28443 calibrator.getInitialBiasYAsAcceleration(by2);
28444 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28445 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28446 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28447 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28448 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28449 final Acceleration bz2 = new Acceleration(0.0,
28450 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28451 calibrator.getInitialBiasZAsAcceleration(bz2);
28452 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28453 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28454 assertEquals(calibrator.getInitialSx(), sx, 0.0);
28455 assertEquals(calibrator.getInitialSy(), sy, 0.0);
28456 assertEquals(calibrator.getInitialSz(), sz, 0.0);
28457 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
28458 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
28459 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
28460 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
28461 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
28462 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
28463 final double[] bias1 = calibrator.getInitialBias();
28464 assertArrayEquals(bias1, bias, 0.0);
28465 final double[] bias2 = new double[3];
28466 calibrator.getInitialBias(bias2);
28467 assertArrayEquals(bias1, bias2, 0.0);
28468 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28469 assertEquals(b1, ba);
28470 final Matrix b2 = new Matrix(3, 1);
28471 calibrator.getInitialBiasAsMatrix(b2);
28472 assertEquals(b1, b2);
28473 final Matrix ma1 = new Matrix(3, 3);
28474 ma1.setSubmatrix(0, 0,
28475 2, 2,
28476 new double[]{ sx, myx, mzx,
28477 mxy, sy, mzy,
28478 mxz, myz, sz});
28479 assertEquals(calibrator.getInitialMa(), ma1);
28480 final Matrix ma2 = new Matrix(3, 3);
28481 calibrator.getInitialMa(ma2);
28482 assertEquals(ma1, ma2);
28483 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28484 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28485 final NEDPosition nedPosition1 = new NEDPosition();
28486 assertTrue(calibrator.getNedPosition(nedPosition1));
28487 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28488 assertNull(calibrator.getMeasurements());
28489 assertTrue(calibrator.isCommonAxisUsed());
28490 assertNull(calibrator.getListener());
28491 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28492 assertFalse(calibrator.isReady());
28493 assertFalse(calibrator.isRunning());
28494 assertNull(calibrator.getEstimatedBiases());
28495 assertFalse(calibrator.getEstimatedBiases(null));
28496 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28497 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28498 assertNull(calibrator.getEstimatedBiasFx());
28499 assertNull(calibrator.getEstimatedBiasFy());
28500 assertNull(calibrator.getEstimatedBiasFz());
28501 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28502 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28503 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28504 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28505 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28506 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28507 assertNull(calibrator.getEstimatedMa());
28508 assertNull(calibrator.getEstimatedSx());
28509 assertNull(calibrator.getEstimatedSy());
28510 assertNull(calibrator.getEstimatedSz());
28511 assertNull(calibrator.getEstimatedMxy());
28512 assertNull(calibrator.getEstimatedMxz());
28513 assertNull(calibrator.getEstimatedMyx());
28514 assertNull(calibrator.getEstimatedMyz());
28515 assertNull(calibrator.getEstimatedMzx());
28516 assertNull(calibrator.getEstimatedMzy());
28517 assertNull(calibrator.getEstimatedCovariance());
28518 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28519
28520
28521 calibrator = null;
28522 try {
28523 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28524 true, new Matrix(1, 1), ma);
28525 fail("IllegalArgumentException expected but not thrown");
28526 } catch (final IllegalArgumentException ignore) {
28527 }
28528 try {
28529 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28530 true, new Matrix(1, 3), ma);
28531 fail("IllegalArgumentException expected but not thrown");
28532 } catch (final IllegalArgumentException ignore) {
28533 }
28534 try {
28535 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28536 true, ba, new Matrix(1, 3));
28537 fail("IllegalArgumentException expected but not thrown");
28538 } catch (final IllegalArgumentException ignore) {
28539 }
28540 try {
28541 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28542 true, ba, new Matrix(3, 1));
28543 fail("IllegalArgumentException expected but not thrown");
28544 } catch (final IllegalArgumentException ignore) {
28545 }
28546 assertNull(calibrator);
28547 }
28548
28549 @Test
28550 public void testConstructor232() throws WrongSizeException {
28551 final Matrix ba = generateBa();
28552 final double[] bias = ba.getBuffer();
28553 final double biasX = ba.getElementAtIndex(0);
28554 final double biasY = ba.getElementAtIndex(1);
28555 final double biasZ = ba.getElementAtIndex(2);
28556
28557 final Matrix ma = generateMaCommonAxis();
28558 final double sx = ma.getElementAt(0, 0);
28559 final double sy = ma.getElementAt(1, 1);
28560 final double sz = ma.getElementAt(2, 2);
28561 final double mxy = ma.getElementAt(0, 1);
28562 final double mxz = ma.getElementAt(0, 2);
28563 final double myx = ma.getElementAt(1, 0);
28564 final double myz = ma.getElementAt(1, 2);
28565 final double mzx = ma.getElementAt(2, 0);
28566 final double mzy = ma.getElementAt(2, 1);
28567
28568 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28569 final double latitude = Math.toRadians(
28570 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28571 final double longitude = Math.toRadians(
28572 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28573 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28574 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28575 final NEDVelocity nedVelocity = new NEDVelocity();
28576 final ECEFPosition ecefPosition = new ECEFPosition();
28577 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28578 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28579 ecefPosition, ecefVelocity);
28580
28581 KnownPositionAccelerometerCalibrator calibrator =
28582 new KnownPositionAccelerometerCalibrator(nedPosition,
28583 true, ba, ma, this);
28584
28585
28586 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28587 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28588 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28589 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28590 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28591 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28592 final Acceleration bx2 = new Acceleration(0.0,
28593 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28594 calibrator.getInitialBiasXAsAcceleration(bx2);
28595 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28596 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28597 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28598 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28599 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28600 final Acceleration by2 = new Acceleration(0.0,
28601 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28602 calibrator.getInitialBiasYAsAcceleration(by2);
28603 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28604 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28605 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28606 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28607 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28608 final Acceleration bz2 = new Acceleration(0.0,
28609 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28610 calibrator.getInitialBiasZAsAcceleration(bz2);
28611 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28612 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28613 assertEquals(calibrator.getInitialSx(), sx, 0.0);
28614 assertEquals(calibrator.getInitialSy(), sy, 0.0);
28615 assertEquals(calibrator.getInitialSz(), sz, 0.0);
28616 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
28617 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
28618 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
28619 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
28620 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
28621 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
28622 final double[] bias1 = calibrator.getInitialBias();
28623 assertArrayEquals(bias1, bias, 0.0);
28624 final double[] bias2 = new double[3];
28625 calibrator.getInitialBias(bias2);
28626 assertArrayEquals(bias1, bias2, 0.0);
28627 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28628 assertEquals(b1, ba);
28629 final Matrix b2 = new Matrix(3, 1);
28630 calibrator.getInitialBiasAsMatrix(b2);
28631 assertEquals(b1, b2);
28632 final Matrix ma1 = new Matrix(3, 3);
28633 ma1.setSubmatrix(0, 0,
28634 2, 2,
28635 new double[]{ sx, myx, mzx,
28636 mxy, sy, mzy,
28637 mxz, myz, sz});
28638 assertEquals(calibrator.getInitialMa(), ma1);
28639 final Matrix ma2 = new Matrix(3, 3);
28640 calibrator.getInitialMa(ma2);
28641 assertEquals(ma1, ma2);
28642 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28643 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28644 final NEDPosition nedPosition1 = new NEDPosition();
28645 assertTrue(calibrator.getNedPosition(nedPosition1));
28646 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28647 assertNull(calibrator.getMeasurements());
28648 assertTrue(calibrator.isCommonAxisUsed());
28649 assertSame(calibrator.getListener(), this);
28650 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28651 assertFalse(calibrator.isReady());
28652 assertFalse(calibrator.isRunning());
28653 assertNull(calibrator.getEstimatedBiases());
28654 assertFalse(calibrator.getEstimatedBiases(null));
28655 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28656 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28657 assertNull(calibrator.getEstimatedBiasFx());
28658 assertNull(calibrator.getEstimatedBiasFy());
28659 assertNull(calibrator.getEstimatedBiasFz());
28660 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28661 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28662 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28663 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28664 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28665 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28666 assertNull(calibrator.getEstimatedMa());
28667 assertNull(calibrator.getEstimatedSx());
28668 assertNull(calibrator.getEstimatedSy());
28669 assertNull(calibrator.getEstimatedSz());
28670 assertNull(calibrator.getEstimatedMxy());
28671 assertNull(calibrator.getEstimatedMxz());
28672 assertNull(calibrator.getEstimatedMyx());
28673 assertNull(calibrator.getEstimatedMyz());
28674 assertNull(calibrator.getEstimatedMzx());
28675 assertNull(calibrator.getEstimatedMzy());
28676 assertNull(calibrator.getEstimatedCovariance());
28677 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28678
28679
28680 calibrator = null;
28681 try {
28682 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28683 true, new Matrix(1, 1), ma, this);
28684 fail("IllegalArgumentException expected but not thrown");
28685 } catch (final IllegalArgumentException ignore) {
28686 }
28687 try {
28688 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28689 true, new Matrix(1, 3), ma, this);
28690 fail("IllegalArgumentException expected but not thrown");
28691 } catch (final IllegalArgumentException ignore) {
28692 }
28693 try {
28694 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28695 true, ba, new Matrix(1, 3), this);
28696 fail("IllegalArgumentException expected but not thrown");
28697 } catch (final IllegalArgumentException ignore) {
28698 }
28699 try {
28700 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28701 true, ba, new Matrix(3, 1), this);
28702 fail("IllegalArgumentException expected but not thrown");
28703 } catch (final IllegalArgumentException ignore) {
28704 }
28705 assertNull(calibrator);
28706 }
28707
28708 @Test
28709 public void testConstructor233() throws WrongSizeException {
28710 final Collection<StandardDeviationBodyKinematics> measurements =
28711 Collections.emptyList();
28712
28713 final Matrix ba = generateBa();
28714 final double[] bias = ba.getBuffer();
28715 final double biasX = ba.getElementAtIndex(0);
28716 final double biasY = ba.getElementAtIndex(1);
28717 final double biasZ = ba.getElementAtIndex(2);
28718
28719 final Matrix ma = generateMaCommonAxis();
28720 final double sx = ma.getElementAt(0, 0);
28721 final double sy = ma.getElementAt(1, 1);
28722 final double sz = ma.getElementAt(2, 2);
28723 final double mxy = ma.getElementAt(0, 1);
28724 final double mxz = ma.getElementAt(0, 2);
28725 final double myx = ma.getElementAt(1, 0);
28726 final double myz = ma.getElementAt(1, 2);
28727 final double mzx = ma.getElementAt(2, 0);
28728 final double mzy = ma.getElementAt(2, 1);
28729
28730 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28731 final double latitude = Math.toRadians(
28732 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28733 final double longitude = Math.toRadians(
28734 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28735 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28736 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28737 final NEDVelocity nedVelocity = new NEDVelocity();
28738 final ECEFPosition ecefPosition = new ECEFPosition();
28739 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28740 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28741 ecefPosition, ecefVelocity);
28742
28743 KnownPositionAccelerometerCalibrator calibrator =
28744 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
28745 true, ba, ma);
28746
28747
28748 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28749 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28750 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28751 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28752 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28753 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28754 final Acceleration bx2 = new Acceleration(0.0,
28755 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28756 calibrator.getInitialBiasXAsAcceleration(bx2);
28757 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28758 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28759 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28760 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28761 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28762 final Acceleration by2 = new Acceleration(0.0,
28763 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28764 calibrator.getInitialBiasYAsAcceleration(by2);
28765 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28766 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28767 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28768 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28769 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28770 final Acceleration bz2 = new Acceleration(0.0,
28771 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28772 calibrator.getInitialBiasZAsAcceleration(bz2);
28773 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28774 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28775 assertEquals(calibrator.getInitialSx(), sx, 0.0);
28776 assertEquals(calibrator.getInitialSy(), sy, 0.0);
28777 assertEquals(calibrator.getInitialSz(), sz, 0.0);
28778 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
28779 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
28780 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
28781 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
28782 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
28783 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
28784 final double[] bias1 = calibrator.getInitialBias();
28785 assertArrayEquals(bias1, bias, 0.0);
28786 final double[] bias2 = new double[3];
28787 calibrator.getInitialBias(bias2);
28788 assertArrayEquals(bias1, bias2, 0.0);
28789 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28790 assertEquals(b1, ba);
28791 final Matrix b2 = new Matrix(3, 1);
28792 calibrator.getInitialBiasAsMatrix(b2);
28793 assertEquals(b1, b2);
28794 final Matrix ma1 = new Matrix(3, 3);
28795 ma1.setSubmatrix(0, 0,
28796 2, 2,
28797 new double[]{ sx, myx, mzx,
28798 mxy, sy, mzy,
28799 mxz, myz, sz});
28800 assertEquals(calibrator.getInitialMa(), ma1);
28801 final Matrix ma2 = new Matrix(3, 3);
28802 calibrator.getInitialMa(ma2);
28803 assertEquals(ma1, ma2);
28804 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28805 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28806 final NEDPosition nedPosition1 = new NEDPosition();
28807 assertTrue(calibrator.getNedPosition(nedPosition1));
28808 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28809 assertSame(calibrator.getMeasurements(), measurements);
28810 assertTrue(calibrator.isCommonAxisUsed());
28811 assertNull(calibrator.getListener());
28812 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28813 assertFalse(calibrator.isReady());
28814 assertFalse(calibrator.isRunning());
28815 assertNull(calibrator.getEstimatedBiases());
28816 assertFalse(calibrator.getEstimatedBiases(null));
28817 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28818 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28819 assertNull(calibrator.getEstimatedBiasFx());
28820 assertNull(calibrator.getEstimatedBiasFy());
28821 assertNull(calibrator.getEstimatedBiasFz());
28822 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28823 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28824 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28825 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28826 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28827 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28828 assertNull(calibrator.getEstimatedMa());
28829 assertNull(calibrator.getEstimatedSx());
28830 assertNull(calibrator.getEstimatedSy());
28831 assertNull(calibrator.getEstimatedSz());
28832 assertNull(calibrator.getEstimatedMxy());
28833 assertNull(calibrator.getEstimatedMxz());
28834 assertNull(calibrator.getEstimatedMyx());
28835 assertNull(calibrator.getEstimatedMyz());
28836 assertNull(calibrator.getEstimatedMzx());
28837 assertNull(calibrator.getEstimatedMzy());
28838 assertNull(calibrator.getEstimatedCovariance());
28839 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28840
28841
28842 calibrator = null;
28843 try {
28844 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28845 measurements, true,
28846 new Matrix(1, 1), ma);
28847 fail("IllegalArgumentException expected but not thrown");
28848 } catch (final IllegalArgumentException ignore) {
28849 }
28850 try {
28851 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28852 measurements, true,
28853 new Matrix(1, 3), ma);
28854 fail("IllegalArgumentException expected but not thrown");
28855 } catch (final IllegalArgumentException ignore) {
28856 }
28857 try {
28858 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28859 measurements, true,
28860 ba, new Matrix(1, 3));
28861 fail("IllegalArgumentException expected but not thrown");
28862 } catch (final IllegalArgumentException ignore) {
28863 }
28864 try {
28865 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28866 measurements, true,
28867 ba, new Matrix(3, 1));
28868 fail("IllegalArgumentException expected but not thrown");
28869 } catch (final IllegalArgumentException ignore) {
28870 }
28871 assertNull(calibrator);
28872 }
28873
28874 @Test
28875 public void testConstructor234() throws WrongSizeException {
28876 final Collection<StandardDeviationBodyKinematics> measurements =
28877 Collections.emptyList();
28878
28879 final Matrix ba = generateBa();
28880 final double[] bias = ba.getBuffer();
28881 final double biasX = ba.getElementAtIndex(0);
28882 final double biasY = ba.getElementAtIndex(1);
28883 final double biasZ = ba.getElementAtIndex(2);
28884
28885 final Matrix ma = generateMaCommonAxis();
28886 final double sx = ma.getElementAt(0, 0);
28887 final double sy = ma.getElementAt(1, 1);
28888 final double sz = ma.getElementAt(2, 2);
28889 final double mxy = ma.getElementAt(0, 1);
28890 final double mxz = ma.getElementAt(0, 2);
28891 final double myx = ma.getElementAt(1, 0);
28892 final double myz = ma.getElementAt(1, 2);
28893 final double mzx = ma.getElementAt(2, 0);
28894 final double mzy = ma.getElementAt(2, 1);
28895
28896 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28897 final double latitude = Math.toRadians(
28898 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28899 final double longitude = Math.toRadians(
28900 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28901 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28902 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28903 final NEDVelocity nedVelocity = new NEDVelocity();
28904 final ECEFPosition ecefPosition = new ECEFPosition();
28905 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28906 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28907 ecefPosition, ecefVelocity);
28908
28909 KnownPositionAccelerometerCalibrator calibrator =
28910 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
28911 true, ba, ma, this);
28912
28913
28914 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28915 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28916 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28917 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28918 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28919 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28920 final Acceleration bx2 = new Acceleration(0.0,
28921 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28922 calibrator.getInitialBiasXAsAcceleration(bx2);
28923 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28924 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28925 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28926 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28927 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28928 final Acceleration by2 = new Acceleration(0.0,
28929 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28930 calibrator.getInitialBiasYAsAcceleration(by2);
28931 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28932 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28933 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28934 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28935 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28936 final Acceleration bz2 = new Acceleration(0.0,
28937 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28938 calibrator.getInitialBiasZAsAcceleration(bz2);
28939 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28940 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28941 assertEquals(calibrator.getInitialSx(), sx, 0.0);
28942 assertEquals(calibrator.getInitialSy(), sy, 0.0);
28943 assertEquals(calibrator.getInitialSz(), sz, 0.0);
28944 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
28945 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
28946 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
28947 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
28948 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
28949 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
28950 final double[] bias1 = calibrator.getInitialBias();
28951 assertArrayEquals(bias1, bias, 0.0);
28952 final double[] bias2 = new double[3];
28953 calibrator.getInitialBias(bias2);
28954 assertArrayEquals(bias1, bias2, 0.0);
28955 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28956 assertEquals(b1, ba);
28957 final Matrix b2 = new Matrix(3, 1);
28958 calibrator.getInitialBiasAsMatrix(b2);
28959 assertEquals(b1, b2);
28960 final Matrix ma1 = new Matrix(3, 3);
28961 ma1.setSubmatrix(0, 0,
28962 2, 2,
28963 new double[]{ sx, myx, mzx,
28964 mxy, sy, mzy,
28965 mxz, myz, sz});
28966 assertEquals(calibrator.getInitialMa(), ma1);
28967 final Matrix ma2 = new Matrix(3, 3);
28968 calibrator.getInitialMa(ma2);
28969 assertEquals(ma1, ma2);
28970 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28971 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28972 final NEDPosition nedPosition1 = new NEDPosition();
28973 assertTrue(calibrator.getNedPosition(nedPosition1));
28974 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28975 assertSame(calibrator.getMeasurements(), measurements);
28976 assertTrue(calibrator.isCommonAxisUsed());
28977 assertSame(calibrator.getListener(), this);
28978 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28979 assertFalse(calibrator.isReady());
28980 assertFalse(calibrator.isRunning());
28981 assertNull(calibrator.getEstimatedBiases());
28982 assertFalse(calibrator.getEstimatedBiases(null));
28983 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28984 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28985 assertNull(calibrator.getEstimatedBiasFx());
28986 assertNull(calibrator.getEstimatedBiasFy());
28987 assertNull(calibrator.getEstimatedBiasFz());
28988 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28989 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28990 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28991 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28992 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28993 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28994 assertNull(calibrator.getEstimatedMa());
28995 assertNull(calibrator.getEstimatedSx());
28996 assertNull(calibrator.getEstimatedSy());
28997 assertNull(calibrator.getEstimatedSz());
28998 assertNull(calibrator.getEstimatedMxy());
28999 assertNull(calibrator.getEstimatedMxz());
29000 assertNull(calibrator.getEstimatedMyx());
29001 assertNull(calibrator.getEstimatedMyz());
29002 assertNull(calibrator.getEstimatedMzx());
29003 assertNull(calibrator.getEstimatedMzy());
29004 assertNull(calibrator.getEstimatedCovariance());
29005 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29006
29007
29008 calibrator = null;
29009 try {
29010 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29011 measurements, true,
29012 new Matrix(1, 1), ma, this);
29013 fail("IllegalArgumentException expected but not thrown");
29014 } catch (final IllegalArgumentException ignore) {
29015 }
29016 try {
29017 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29018 measurements, true,
29019 new Matrix(1, 3), ma, this);
29020 fail("IllegalArgumentException expected but not thrown");
29021 } catch (final IllegalArgumentException ignore) {
29022 }
29023 try {
29024 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29025 measurements, true,
29026 ba, new Matrix(1, 3), this);
29027 fail("IllegalArgumentException expected but not thrown");
29028 } catch (final IllegalArgumentException ignore) {
29029 }
29030 try {
29031 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29032 measurements, true,
29033 ba, new Matrix(3, 1), this);
29034 fail("IllegalArgumentException expected but not thrown");
29035 } catch (final IllegalArgumentException ignore) {
29036 }
29037 assertNull(calibrator);
29038 }
29039
29040 @Test
29041 public void testGetSetInitialBiasX() throws LockedException {
29042 final KnownPositionAccelerometerCalibrator calibrator =
29043 new KnownPositionAccelerometerCalibrator();
29044
29045 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
29046
29047
29048 final Matrix ba = generateBa();
29049 final double biasX = ba.getElementAtIndex(0);
29050
29051 calibrator.setInitialBiasX(biasX);
29052
29053
29054 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29055 }
29056
29057 @Test
29058 public void testGetSetInitialBiasY() throws LockedException {
29059 final KnownPositionAccelerometerCalibrator calibrator =
29060 new KnownPositionAccelerometerCalibrator();
29061
29062 assertEquals(calibrator.getInitialBiasY(),0.0, 0.0);
29063
29064
29065 final Matrix ba = generateBa();
29066 final double biasY = ba.getElementAtIndex(1);
29067
29068 calibrator.setInitialBiasY(biasY);
29069
29070
29071 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29072 }
29073
29074 @Test
29075 public void testGetSetInitialBiasZ() throws LockedException {
29076 final KnownPositionAccelerometerCalibrator calibrator =
29077 new KnownPositionAccelerometerCalibrator();
29078
29079 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
29080
29081
29082 final Matrix ba = generateBa();
29083 final double biasZ = ba.getElementAtIndex(2);
29084
29085 calibrator.setInitialBiasZ(biasZ);
29086
29087
29088 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29089 }
29090
29091 @Test
29092 public void testGetSetInitialBiasXAsAcceleration() throws LockedException {
29093 final KnownPositionAccelerometerCalibrator calibrator =
29094 new KnownPositionAccelerometerCalibrator();
29095
29096
29097 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29098
29099 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
29100 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29101
29102
29103 final Matrix ba = generateBa();
29104 final double biasX = ba.getElementAtIndex(0);
29105
29106 final Acceleration bx2 = new Acceleration(biasX,
29107 AccelerationUnit.METERS_PER_SQUARED_SECOND);
29108 calibrator.setInitialBiasX(bx2);
29109
29110
29111 final Acceleration bx3 = calibrator.getInitialBiasXAsAcceleration();
29112 final Acceleration bx4 = new Acceleration(biasX,
29113 AccelerationUnit.METERS_PER_SQUARED_SECOND);
29114 calibrator.getInitialBiasXAsAcceleration(bx4);
29115
29116 assertEquals(bx3, bx4);
29117 }
29118
29119 @Test
29120 public void testGetSetInitialBiasYAsAcceleration() throws LockedException {
29121 final KnownPositionAccelerometerCalibrator calibrator =
29122 new KnownPositionAccelerometerCalibrator();
29123
29124
29125 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29126
29127 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
29128 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29129
29130
29131 final Matrix ba = generateBa();
29132 final double biasY = ba.getElementAtIndex(1);
29133
29134 final Acceleration by2 = new Acceleration(biasY,
29135 AccelerationUnit.METERS_PER_SQUARED_SECOND);
29136 calibrator.setInitialBiasY(by2);
29137
29138
29139 final Acceleration by3 = calibrator.getInitialBiasYAsAcceleration();
29140 final Acceleration by4 = new Acceleration(biasY,
29141 AccelerationUnit.METERS_PER_SQUARED_SECOND);
29142 calibrator.getInitialBiasYAsAcceleration(by4);
29143
29144 assertEquals(by3, by4);
29145 }
29146
29147 @Test
29148 public void testGetSetInitialBiasZAsAcceleration() throws LockedException {
29149 final KnownPositionAccelerometerCalibrator calibrator =
29150 new KnownPositionAccelerometerCalibrator();
29151
29152
29153 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29154
29155 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
29156 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29157
29158
29159 final Matrix ba = generateBa();
29160 final double biasZ = ba.getElementAtIndex(2);
29161
29162 final Acceleration bz2 = new Acceleration(biasZ,
29163 AccelerationUnit.METERS_PER_SQUARED_SECOND);
29164 calibrator.setInitialBiasZ(bz2);
29165
29166
29167 final Acceleration bz3 = calibrator.getInitialBiasZAsAcceleration();
29168 final Acceleration bz4 = new Acceleration(biasZ,
29169 AccelerationUnit.METERS_PER_SQUARED_SECOND);
29170 calibrator.getInitialBiasZAsAcceleration(bz4);
29171
29172 assertEquals(bz3, bz4);
29173 }
29174
29175 @Test
29176 public void testSetInitialBias1() throws LockedException {
29177 final KnownPositionAccelerometerCalibrator calibrator =
29178 new KnownPositionAccelerometerCalibrator();
29179
29180
29181 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
29182 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
29183 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
29184
29185
29186 final Matrix ba = generateBa();
29187 final double biasX = ba.getElementAtIndex(0);
29188 final double biasY = ba.getElementAtIndex(1);
29189 final double biasZ = ba.getElementAtIndex(2);
29190
29191 calibrator.setInitialBias(biasX, biasY, biasZ);
29192
29193
29194 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29195 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29196 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29197 }
29198
29199 @Test
29200 public void testSetInitialBias2() throws LockedException {
29201 final KnownPositionAccelerometerCalibrator calibrator =
29202 new KnownPositionAccelerometerCalibrator();
29203
29204
29205 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
29206 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
29207 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
29208
29209
29210 final Matrix ba = generateBa();
29211 final double biasX = ba.getElementAtIndex(0);
29212 final double biasY = ba.getElementAtIndex(1);
29213 final double biasZ = ba.getElementAtIndex(2);
29214
29215 final Acceleration bx = new Acceleration(biasX,
29216 AccelerationUnit.METERS_PER_SQUARED_SECOND);
29217 final Acceleration by = new Acceleration(biasY,
29218 AccelerationUnit.METERS_PER_SQUARED_SECOND);
29219 final Acceleration bz = new Acceleration(biasZ,
29220 AccelerationUnit.METERS_PER_SQUARED_SECOND);
29221
29222 calibrator.setInitialBias(bx, by, bz);
29223
29224
29225 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29226 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29227 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29228 }
29229
29230 @Test
29231 public void testGetSetInitialSx() throws WrongSizeException, LockedException {
29232 final KnownPositionAccelerometerCalibrator calibrator =
29233 new KnownPositionAccelerometerCalibrator();
29234
29235 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29236
29237
29238 final Matrix ma = generateMaGeneral();
29239 final double sx = ma.getElementAt(0, 0);
29240
29241 calibrator.setInitialSx(sx);
29242
29243
29244 assertEquals(calibrator.getInitialSx(), sx, 0.0);
29245 }
29246
29247 @Test
29248 public void testGetSetInitialSy() throws WrongSizeException, LockedException {
29249 final KnownPositionAccelerometerCalibrator calibrator =
29250 new KnownPositionAccelerometerCalibrator();
29251
29252 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29253
29254
29255 final Matrix ma = generateMaGeneral();
29256 final double sy = ma.getElementAt(1, 1);
29257
29258 calibrator.setInitialSy(sy);
29259
29260
29261 assertEquals(calibrator.getInitialSy(), sy, 0.0);
29262 }
29263
29264 @Test
29265 public void testGetSetInitialSz() throws WrongSizeException, LockedException {
29266 final KnownPositionAccelerometerCalibrator calibrator =
29267 new KnownPositionAccelerometerCalibrator();
29268
29269 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29270
29271
29272 final Matrix ma = generateMaGeneral();
29273 final double sz = ma.getElementAt(2, 2);
29274
29275 calibrator.setInitialSz(sz);
29276
29277
29278 assertEquals(calibrator.getInitialSz(), sz, 0.0);
29279 }
29280
29281 @Test
29282 public void testGetSetInitialMxy() throws WrongSizeException, LockedException {
29283 final KnownPositionAccelerometerCalibrator calibrator =
29284 new KnownPositionAccelerometerCalibrator();
29285
29286 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29287
29288
29289 final Matrix ma = generateMaGeneral();
29290 final double mxy = ma.getElementAt(0, 1);
29291
29292 calibrator.setInitialMxy(mxy);
29293
29294
29295 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
29296 }
29297
29298 @Test
29299 public void testGetSetInitialMxz() throws WrongSizeException, LockedException {
29300 final KnownPositionAccelerometerCalibrator calibrator =
29301 new KnownPositionAccelerometerCalibrator();
29302
29303 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29304
29305
29306 final Matrix ma = generateMaGeneral();
29307 final double mxz = ma.getElementAt(0, 2);
29308
29309 calibrator.setInitialMxz(mxz);
29310
29311
29312 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
29313 }
29314
29315 @Test
29316 public void testGetSetInitialMyx() throws WrongSizeException, LockedException {
29317 final KnownPositionAccelerometerCalibrator calibrator =
29318 new KnownPositionAccelerometerCalibrator();
29319
29320 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29321
29322
29323 final Matrix ma = generateMaGeneral();
29324 final double myx = ma.getElementAt(1, 0);
29325
29326 calibrator.setInitialMyx(myx);
29327
29328
29329 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
29330 }
29331
29332 @Test
29333 public void testGetSetInitialMyz() throws WrongSizeException, LockedException {
29334 final KnownPositionAccelerometerCalibrator calibrator =
29335 new KnownPositionAccelerometerCalibrator();
29336
29337 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29338
29339
29340 final Matrix ma = generateMaGeneral();
29341 final double myz = ma.getElementAt(1, 2);
29342
29343 calibrator.setInitialMyz(myz);
29344
29345
29346 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
29347 }
29348
29349 @Test
29350 public void testGetSetInitialMzx() throws WrongSizeException, LockedException {
29351 final KnownPositionAccelerometerCalibrator calibrator =
29352 new KnownPositionAccelerometerCalibrator();
29353
29354 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29355
29356
29357 final Matrix ma = generateMaGeneral();
29358 final double mzx = ma.getElementAt(2, 0);
29359
29360 calibrator.setInitialMzx(mzx);
29361
29362
29363 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
29364 }
29365
29366 @Test
29367 public void testGetSetInitialMzy() throws WrongSizeException, LockedException {
29368 final KnownPositionAccelerometerCalibrator calibrator =
29369 new KnownPositionAccelerometerCalibrator();
29370
29371 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29372
29373
29374 final Matrix ma = generateMaGeneral();
29375 final double mzy = ma.getElementAt(2, 1);
29376
29377 calibrator.setInitialMzy(mzy);
29378
29379
29380 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
29381 }
29382
29383 @Test
29384 public void testSetInitialScalingFactors()
29385 throws WrongSizeException, LockedException {
29386
29387 final KnownPositionAccelerometerCalibrator calibrator =
29388 new KnownPositionAccelerometerCalibrator();
29389
29390
29391 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29392 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29393 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29394
29395
29396 final Matrix ma = generateMaGeneral();
29397 final double sx = ma.getElementAt(0, 0);
29398 final double sy = ma.getElementAt(1, 1);
29399 final double sz = ma.getElementAt(2, 2);
29400
29401 calibrator.setInitialScalingFactors(sx, sy, sz);
29402
29403
29404 assertEquals(calibrator.getInitialSx(), sx, 0.0);
29405 assertEquals(calibrator.getInitialSy(), sy, 0.0);
29406 assertEquals(calibrator.getInitialSz(), sz, 0.0);
29407 }
29408
29409 @Test
29410 public void testSetInitialCrossCouplingErrors()
29411 throws WrongSizeException, LockedException {
29412
29413 final KnownPositionAccelerometerCalibrator calibrator =
29414 new KnownPositionAccelerometerCalibrator();
29415
29416
29417 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29418 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29419 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29420 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29421 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29422 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29423
29424
29425 final Matrix ma = generateMaGeneral();
29426 final double mxy = ma.getElementAt(0, 1);
29427 final double mxz = ma.getElementAt(0, 2);
29428 final double myx = ma.getElementAt(1, 0);
29429 final double myz = ma.getElementAt(1, 2);
29430 final double mzx = ma.getElementAt(2, 0);
29431 final double mzy = ma.getElementAt(2, 1);
29432
29433 calibrator.setInitialCrossCouplingErrors(mxy, mxz, myx, myz, mzx, mzy);
29434
29435
29436 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
29437 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
29438 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
29439 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
29440 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
29441 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
29442 }
29443
29444 @Test
29445 public void testSetInitialScalingFactorsAndCrossCouplingErrors()
29446 throws WrongSizeException, LockedException {
29447
29448 final KnownPositionAccelerometerCalibrator calibrator =
29449 new KnownPositionAccelerometerCalibrator();
29450
29451
29452 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29453 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29454 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29455 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29456 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29457 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29458 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29459 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29460 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29461
29462
29463 final Matrix ma = generateMaGeneral();
29464 final double sx = ma.getElementAt(0, 0);
29465 final double sy = ma.getElementAt(1, 1);
29466 final double sz = ma.getElementAt(2, 2);
29467 final double mxy = ma.getElementAt(0, 1);
29468 final double mxz = ma.getElementAt(0, 2);
29469 final double myx = ma.getElementAt(1, 0);
29470 final double myz = ma.getElementAt(1, 2);
29471 final double mzx = ma.getElementAt(2, 0);
29472 final double mzy = ma.getElementAt(2, 1);
29473
29474 calibrator.setInitialScalingFactorsAndCrossCouplingErrors(sx, sy, sz,
29475 mxy, mxz, myx, myz, mzx, mzy);
29476
29477
29478 assertEquals(calibrator.getInitialSx(), sx, 0.0);
29479 assertEquals(calibrator.getInitialSy(), sy, 0.0);
29480 assertEquals(calibrator.getInitialSz(), sz, 0.0);
29481 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
29482 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
29483 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
29484 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
29485 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
29486 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
29487 }
29488
29489 @Test
29490 public void testGetSetInitialBias() throws LockedException {
29491 final KnownPositionAccelerometerCalibrator calibrator =
29492 new KnownPositionAccelerometerCalibrator();
29493
29494
29495 final double[] bias1 = calibrator.getInitialBias();
29496 assertArrayEquals(bias1, new double[3], 0.0);
29497
29498
29499 final Matrix ba = generateBa();
29500 final double[] bias2 = ba.getBuffer();
29501 calibrator.setInitialBias(bias2);
29502
29503
29504 final double[] bias3 = calibrator.getInitialBias();
29505 final double[] bias4 = new double[3];
29506 calibrator.getInitialBias(bias4);
29507
29508 assertArrayEquals(bias2, bias3, 0.0);
29509 assertArrayEquals(bias2, bias4, 0.0);
29510
29511
29512 try {
29513 calibrator.getInitialBias(new double[1]);
29514 fail("IllegalArgumentException expected but not thrown");
29515 } catch(final IllegalArgumentException ignore) {
29516 }
29517 try {
29518 calibrator.setInitialBias(new double[1]);
29519 fail("IllegalArgumentException expected but not thrown");
29520 } catch(final IllegalArgumentException ignore) {
29521 }
29522 }
29523
29524 @Test
29525 public void testGetSetInitialBiasAsMatrix() throws WrongSizeException,
29526 LockedException {
29527
29528 final KnownPositionAccelerometerCalibrator calibrator =
29529 new KnownPositionAccelerometerCalibrator();
29530
29531
29532 final Matrix bias1 = calibrator.getInitialBiasAsMatrix();
29533 assertEquals(bias1, new Matrix(3, 1));
29534
29535
29536 final Matrix bias2 = generateBa();
29537 calibrator.setInitialBias(bias2);
29538
29539
29540 final Matrix bias3 = calibrator.getInitialBiasAsMatrix();
29541 final Matrix bias4 = new Matrix(3, 1);
29542 calibrator.getInitialBiasAsMatrix(bias4);
29543
29544 assertEquals(bias2, bias3);
29545 assertEquals(bias2, bias4);
29546
29547
29548 try {
29549 calibrator.getInitialBiasAsMatrix(new Matrix(1, 1));
29550 fail("IllegalArgumentException expected but not thrown");
29551 } catch (final IllegalArgumentException ignore) {
29552 }
29553 try {
29554 calibrator.getInitialBiasAsMatrix(new Matrix(3, 3));
29555 fail("IllegalArgumentException expected but not thrown");
29556 } catch (final IllegalArgumentException ignore) {
29557 }
29558 try {
29559 calibrator.setInitialBias(new Matrix(1, 1));
29560 fail("IllegalArgumentException expected but not thrown");
29561 } catch (final IllegalArgumentException ignore) {
29562 }
29563 try {
29564 calibrator.setInitialBias(new Matrix(3, 3));
29565 fail("IllegalArgumentException expected but not thrown");
29566 } catch (final IllegalArgumentException ignore) {
29567 }
29568 }
29569
29570 @Test
29571 public void testGetSetInitialMa() throws WrongSizeException, LockedException {
29572 final KnownPositionAccelerometerCalibrator calibrator =
29573 new KnownPositionAccelerometerCalibrator();
29574
29575
29576 final Matrix ma1 = calibrator.getInitialMa();
29577 assertEquals(ma1, new Matrix(3, 3));
29578
29579
29580 final Matrix ma2 = generateMaGeneral();
29581 calibrator.setInitialMa(ma2);
29582
29583
29584 final Matrix ma3 = calibrator.getInitialMa();
29585 final Matrix ma4 = new Matrix(3, 3);
29586 calibrator.getInitialMa(ma4);
29587
29588 assertEquals(ma2, ma3);
29589 assertEquals(ma2, ma4);
29590
29591
29592 try {
29593 calibrator.getInitialMa(new Matrix(1, 3));
29594 fail("IllegalArgumentException expected but not thrown");
29595 } catch (final IllegalArgumentException ignore) {
29596 }
29597 try {
29598 calibrator.getInitialMa(new Matrix(3, 1));
29599 fail("IllegalArgumentException expected but not thrown");
29600 } catch (final IllegalArgumentException ignore) {
29601 }
29602 try {
29603 calibrator.setInitialMa(new Matrix(1, 3));
29604 fail("IllegalArgumentException expected but not thrown");
29605 } catch (final IllegalArgumentException ignore) {
29606 }
29607 try {
29608 calibrator.setInitialMa(new Matrix(3, 1));
29609 fail("IllegalArgumentException expected but not thrown");
29610 } catch (final IllegalArgumentException ignore) {
29611 }
29612 }
29613
29614 @Test
29615 public void testGetSetEcefPosition() throws LockedException {
29616 final KnownPositionAccelerometerCalibrator calibrator =
29617 new KnownPositionAccelerometerCalibrator();
29618
29619
29620 assertNull(calibrator.getEcefPosition());
29621
29622
29623 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29624 final double latitude = Math.toRadians(
29625 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29626 final double longitude = Math.toRadians(
29627 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29628 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29629 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29630 final NEDVelocity nedVelocity = new NEDVelocity();
29631 final ECEFPosition ecefPosition = new ECEFPosition();
29632 final ECEFVelocity ecefVelocity = new ECEFVelocity();
29633 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29634 ecefPosition, ecefVelocity);
29635
29636 calibrator.setPosition(ecefPosition);
29637
29638
29639 assertSame(calibrator.getEcefPosition(), ecefPosition);
29640 }
29641
29642 @Test
29643 public void testGetSetNedPosition() throws LockedException {
29644 final KnownPositionAccelerometerCalibrator calibrator =
29645 new KnownPositionAccelerometerCalibrator();
29646
29647
29648 assertNull(calibrator.getNedPosition());
29649 assertFalse(calibrator.getNedPosition(null));
29650
29651
29652 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29653 final double latitude = Math.toRadians(
29654 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29655 final double longitude = Math.toRadians(
29656 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29657 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29658 final NEDPosition nedPosition1 = new NEDPosition(latitude, longitude, height);
29659
29660 calibrator.setPosition(nedPosition1);
29661
29662
29663 final NEDPosition nedPosition2 = calibrator.getNedPosition();
29664 final NEDPosition nedPosition3 = new NEDPosition();
29665 calibrator.getNedPosition(nedPosition3);
29666
29667 assertTrue(nedPosition1.equals(nedPosition2, ABSOLUTE_ERROR));
29668 assertTrue(nedPosition1.equals(nedPosition3, ABSOLUTE_ERROR));
29669 }
29670
29671 @Test
29672 public void testGetSetMeasurements() throws LockedException {
29673 final KnownPositionAccelerometerCalibrator calibrator =
29674 new KnownPositionAccelerometerCalibrator();
29675
29676
29677 assertNull(calibrator.getMeasurements());
29678
29679
29680 final Collection<StandardDeviationBodyKinematics> measurements =
29681 Collections.emptyList();
29682 calibrator.setMeasurements(measurements);
29683
29684
29685 assertSame(calibrator.getMeasurements(), measurements);
29686 }
29687
29688 @Test
29689 public void testIsSetCommonAxisUsed() throws LockedException {
29690 final KnownPositionAccelerometerCalibrator calibrator =
29691 new KnownPositionAccelerometerCalibrator();
29692
29693
29694 assertFalse(calibrator.isCommonAxisUsed());
29695
29696
29697 calibrator.setCommonAxisUsed(true);
29698
29699
29700 assertTrue(calibrator.isCommonAxisUsed());
29701 }
29702
29703 @Test
29704 public void testGetSetListener() throws LockedException {
29705 final KnownPositionAccelerometerCalibrator calibrator =
29706 new KnownPositionAccelerometerCalibrator();
29707
29708
29709 assertNull(calibrator.getListener());
29710
29711
29712 calibrator.setListener(this);
29713
29714
29715 assertSame(calibrator.getListener(), this);
29716 }
29717
29718 @Test
29719 public void testIsReady() throws LockedException {
29720 final KnownPositionAccelerometerCalibrator calibrator =
29721 new KnownPositionAccelerometerCalibrator();
29722
29723
29724 assertFalse(calibrator.isReady());
29725
29726
29727
29728 final Collection<StandardDeviationBodyKinematics> measurements1 =
29729 Collections.emptyList();
29730 calibrator.setMeasurements(measurements1);
29731
29732
29733 assertFalse(calibrator.isReady());
29734
29735
29736
29737 calibrator.setCommonAxisUsed(false);
29738
29739 final List<StandardDeviationBodyKinematics> measurements2 = new ArrayList<>();
29740 for (int i = 0; i < KnownPositionAccelerometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL; i++) {
29741 measurements2.add(new StandardDeviationBodyKinematics());
29742 }
29743 calibrator.setMeasurements(measurements2);
29744
29745
29746 assertFalse(calibrator.isReady());
29747
29748
29749
29750 final ECEFPosition position = new ECEFPosition();
29751 calibrator.setPosition(position);
29752
29753 assertTrue(calibrator.isReady());
29754
29755
29756
29757 measurements2.clear();
29758 for (int i = 0; i < KnownPositionAccelerometerCalibrator.MINIMUM_MEASUREMENTS_COMON_Z_AXIS; i++) {
29759 measurements2.add(new StandardDeviationBodyKinematics());
29760 }
29761 calibrator.setMeasurements(measurements2);
29762
29763
29764 assertFalse(calibrator.isReady());
29765
29766 calibrator.setCommonAxisUsed(true);
29767
29768 assertTrue(calibrator.isReady());
29769 }
29770
29771 @Test
29772 public void testCalibrateForGeneralCaseWithMinimumMeasuresAndNoNoise()
29773 throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
29774 LockedException, NotReadyException {
29775
29776 int numValid = 0;
29777 for (int t = 0; t < TIMES; t++) {
29778 final Matrix ba = generateBa();
29779 final Matrix bg = generateBg();
29780 final Matrix ma = generateMaGeneral();
29781 final Matrix mg = generateMg();
29782 final Matrix gg = generateGg();
29783
29784
29785 final double accelNoiseRootPSD = 0.0;
29786 final double gyroNoiseRootPSD = 0.0;
29787 final double accelQuantLevel = 0.0;
29788 final double gyroQuantLevel = 0.0;
29789
29790 final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
29791 gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
29792
29793 final Random random = new Random();
29794 final UniformRandomizer randomizer = new UniformRandomizer(random);
29795 final double latitude = Math.toRadians(
29796 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29797 final double longitude = Math.toRadians(
29798 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29799 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29800 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29801
29802 final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
29803 final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
29804 final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
29805
29806 final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
29807 for (int i = 0; i < KnownPositionAccelerometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL; i++) {
29808 final double roll = Math.toRadians(
29809 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29810 final double pitch = Math.toRadians(
29811 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29812 final double yaw = Math.toRadians(
29813 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29814 final CoordinateTransformation nedC = new CoordinateTransformation(
29815 roll, pitch, yaw, FrameType.BODY_FRAME,
29816 FrameType.LOCAL_NAVIGATION_FRAME);
29817
29818 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
29819 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
29820 .convertNEDtoECEFAndReturnNew(nedFrame);
29821
29822
29823
29824 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
29825 .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
29826 ecefFrame);
29827
29828
29829
29830 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
29831 .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
29832
29833 final StandardDeviationBodyKinematics measurement =
29834 new StandardDeviationBodyKinematics(measuredKinematics,
29835 specificForceStandardDeviation,
29836 angularRateStandardDeviation);
29837 measurements.add(measurement);
29838 }
29839
29840
29841
29842 final KnownPositionAccelerometerCalibrator calibrator =
29843 new KnownPositionAccelerometerCalibrator(nedPosition,
29844 measurements, false, ba, ma, this);
29845
29846
29847 reset();
29848 assertTrue(calibrator.isReady());
29849 assertFalse(calibrator.isRunning());
29850 assertEquals(mCalibrateStart, 0);
29851 assertEquals(mCalibrateEnd, 0);
29852
29853 try {
29854 calibrator.calibrate();
29855 } catch (final CalibrationException e) {
29856 continue;
29857 }
29858
29859
29860 assertTrue(calibrator.isReady());
29861 assertFalse(calibrator.isRunning());
29862 assertEquals(mCalibrateStart, 1);
29863 assertEquals(mCalibrateEnd, 1);
29864
29865 final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
29866 final Matrix estimatedMa = calibrator.getEstimatedMa();
29867
29868
29869
29870
29871 if (!ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR)) {
29872 continue;
29873 }
29874 if (!ma.equals(estimatedMa, 3.0 * LARGE_ABSOLUTE_ERROR)) {
29875 continue;
29876 }
29877
29878 assertTrue(ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR));
29879 assertTrue(ma.equals(estimatedMa, 3.0 * LARGE_ABSOLUTE_ERROR));
29880
29881 numValid++;
29882
29883 break;
29884 }
29885
29886 assertTrue(numValid > 0);
29887 }
29888
29889 @Test
29890 public void testCalibrateForGeneralCaseWithLargeNumberOfMeasurementsAndNoise()
29891 throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
29892 LockedException, NotReadyException {
29893
29894 int numValid = 0;
29895 for (int t = 0; t < TIMES; t++) {
29896 final Matrix ba = generateBa();
29897 final Matrix bg = generateBg();
29898 final Matrix ma = generateMaGeneral();
29899 final Matrix mg = generateMg();
29900 final Matrix gg = generateGg();
29901
29902
29903 final double accelNoiseRootPSD = getAccelNoiseRootPSD();
29904 final double gyroNoiseRootPSD = getGyroNoiseRootPSD();
29905 final double accelQuantLevel = 0.0;
29906 final double gyroQuantLevel = 0.0;
29907
29908 final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
29909 gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
29910
29911 final Random random = new Random();
29912 final UniformRandomizer randomizer = new UniformRandomizer(random);
29913 final double latitude = Math.toRadians(
29914 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29915 final double longitude = Math.toRadians(
29916 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29917 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29918 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29919
29920 final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
29921 final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
29922 final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
29923
29924 final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
29925 for (int i = 0; i < LARGE_MEASUREMENT_NUMBER; i++) {
29926 final double roll = Math.toRadians(
29927 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29928 final double pitch = Math.toRadians(
29929 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29930 final double yaw = Math.toRadians(
29931 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29932 final CoordinateTransformation nedC = new CoordinateTransformation(
29933 roll, pitch, yaw, FrameType.BODY_FRAME,
29934 FrameType.LOCAL_NAVIGATION_FRAME);
29935
29936 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
29937 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
29938 .convertNEDtoECEFAndReturnNew(nedFrame);
29939
29940
29941
29942 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
29943 .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
29944 ecefFrame);
29945
29946
29947
29948 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
29949 .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
29950
29951 final StandardDeviationBodyKinematics measurement =
29952 new StandardDeviationBodyKinematics(measuredKinematics,
29953 specificForceStandardDeviation,
29954 angularRateStandardDeviation);
29955 measurements.add(measurement);
29956 }
29957
29958
29959
29960 final KnownPositionAccelerometerCalibrator calibrator =
29961 new KnownPositionAccelerometerCalibrator(nedPosition,
29962 measurements, false, ba, ma, this);
29963
29964
29965 reset();
29966 assertTrue(calibrator.isReady());
29967 assertFalse(calibrator.isRunning());
29968 assertEquals(mCalibrateStart, 0);
29969 assertEquals(mCalibrateEnd, 0);
29970
29971 try {
29972 calibrator.calibrate();
29973 } catch (final CalibrationException e) {
29974 continue;
29975 }
29976
29977
29978 assertTrue(calibrator.isReady());
29979 assertFalse(calibrator.isRunning());
29980 assertEquals(mCalibrateStart, 1);
29981 assertEquals(mCalibrateEnd, 1);
29982
29983 final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
29984 final Matrix estimatedMa = calibrator.getEstimatedMa();
29985
29986
29987
29988
29989 if (!ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR)) {
29990 continue;
29991 }
29992 if (!ma.equals(estimatedMa, 3.0 * LARGE_ABSOLUTE_ERROR)) {
29993 continue;
29994 }
29995
29996 assertTrue(ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR));
29997 assertTrue(ma.equals(estimatedMa, 3.0 * LARGE_ABSOLUTE_ERROR));
29998
29999 numValid++;
30000
30001 break;
30002 }
30003
30004 assertTrue(numValid > 0);
30005 }
30006
30007
30008 @Test
30009 public void testCalibrateForCommonAxisCaseWithMinimumMeasuresAndNoNoise()
30010 throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
30011 LockedException, NotReadyException {
30012
30013 int numValid = 0;
30014 for (int t = 0; t < TIMES; t++) {
30015 final Matrix ba = generateBa();
30016 final Matrix bg = generateBg();
30017 final Matrix ma = generateMaCommonAxis();
30018 final Matrix mg = generateMg();
30019 final Matrix gg = generateGg();
30020
30021
30022 final double accelNoiseRootPSD = 0.0;
30023 final double gyroNoiseRootPSD = 0.0;
30024 final double accelQuantLevel = 0.0;
30025 final double gyroQuantLevel = 0.0;
30026
30027 final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
30028 gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
30029
30030 final Random random = new Random();
30031 final UniformRandomizer randomizer = new UniformRandomizer(random);
30032 final double latitude = Math.toRadians(
30033 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
30034 final double longitude = Math.toRadians(
30035 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30036 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30037 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30038
30039 final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
30040 final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
30041 final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
30042
30043 final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
30044 for (int i = 0; i < KnownPositionAccelerometerCalibrator.MINIMUM_MEASUREMENTS_COMON_Z_AXIS; i++) {
30045 final double roll = Math.toRadians(
30046 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
30047 final double pitch = Math.toRadians(
30048 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
30049 final double yaw = Math.toRadians(
30050 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
30051 final CoordinateTransformation nedC = new CoordinateTransformation(
30052 roll, pitch, yaw, FrameType.BODY_FRAME,
30053 FrameType.LOCAL_NAVIGATION_FRAME);
30054
30055 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
30056 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
30057 .convertNEDtoECEFAndReturnNew(nedFrame);
30058
30059
30060
30061 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
30062 .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
30063 ecefFrame);
30064
30065
30066
30067 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
30068 .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
30069
30070 final StandardDeviationBodyKinematics measurement =
30071 new StandardDeviationBodyKinematics(measuredKinematics,
30072 specificForceStandardDeviation,
30073 angularRateStandardDeviation);
30074 measurements.add(measurement);
30075 }
30076
30077
30078
30079 final KnownPositionAccelerometerCalibrator calibrator =
30080 new KnownPositionAccelerometerCalibrator(nedPosition,
30081 measurements, true, ba, ma, this);
30082
30083
30084 reset();
30085 assertTrue(calibrator.isReady());
30086 assertFalse(calibrator.isRunning());
30087 assertEquals(mCalibrateStart, 0);
30088 assertEquals(mCalibrateEnd, 0);
30089
30090 try {
30091 calibrator.calibrate();
30092 } catch (final CalibrationException e) {
30093 continue;
30094 }
30095
30096
30097 assertTrue(calibrator.isReady());
30098 assertFalse(calibrator.isRunning());
30099 assertEquals(mCalibrateStart, 1);
30100 assertEquals(mCalibrateEnd, 1);
30101
30102 final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
30103 final Matrix estimatedMa = calibrator.getEstimatedMa();
30104
30105
30106
30107
30108 if (!ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR)) {
30109 continue;
30110 }
30111 if (!ma.equals(estimatedMa, 20.0 * LARGE_ABSOLUTE_ERROR)) {
30112 continue;
30113 }
30114
30115 assertTrue(ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR));
30116 assertTrue(ma.equals(estimatedMa, 20.0 * LARGE_ABSOLUTE_ERROR));
30117
30118 numValid++;
30119
30120 break;
30121 }
30122
30123 assertTrue(numValid > 0);
30124 }
30125
30126 @Test
30127 public void testCalibrateForCommonAxisCaseWithLargeNumberOfMeasurementsAndNoise()
30128 throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
30129 LockedException, NotReadyException {
30130
30131 int numValid = 0;
30132 for (int t = 0; t < TIMES; t++) {
30133 final Matrix ba = generateBa();
30134 final Matrix bg = generateBg();
30135 final Matrix ma = generateMaCommonAxis();
30136 final Matrix mg = generateMg();
30137 final Matrix gg = generateGg();
30138
30139
30140 final double accelNoiseRootPSD = getAccelNoiseRootPSD();
30141 final double gyroNoiseRootPSD = getGyroNoiseRootPSD();
30142 final double accelQuantLevel = 0.0;
30143 final double gyroQuantLevel = 0.0;
30144
30145 final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
30146 gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
30147
30148 final Random random = new Random();
30149 final UniformRandomizer randomizer = new UniformRandomizer(random);
30150 final double latitude = Math.toRadians(
30151 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
30152 final double longitude = Math.toRadians(
30153 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30154 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30155 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30156
30157 final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
30158 final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
30159 final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
30160
30161 final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
30162 for (int i = 0; i < LARGE_MEASUREMENT_NUMBER; i++) {
30163 final double roll = Math.toRadians(
30164 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
30165 final double pitch = Math.toRadians(
30166 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
30167 final double yaw = Math.toRadians(
30168 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
30169 final CoordinateTransformation nedC = new CoordinateTransformation(
30170 roll, pitch, yaw, FrameType.BODY_FRAME,
30171 FrameType.LOCAL_NAVIGATION_FRAME);
30172
30173 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
30174 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
30175 .convertNEDtoECEFAndReturnNew(nedFrame);
30176
30177
30178
30179 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
30180 .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
30181 ecefFrame);
30182
30183
30184
30185 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
30186 .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
30187
30188 final StandardDeviationBodyKinematics measurement =
30189 new StandardDeviationBodyKinematics(measuredKinematics,
30190 specificForceStandardDeviation,
30191 angularRateStandardDeviation);
30192 measurements.add(measurement);
30193 }
30194
30195
30196
30197 final KnownPositionAccelerometerCalibrator calibrator =
30198 new KnownPositionAccelerometerCalibrator(nedPosition,
30199 measurements, true, ba, ma, this);
30200
30201
30202 reset();
30203 assertTrue(calibrator.isReady());
30204 assertFalse(calibrator.isRunning());
30205 assertEquals(mCalibrateStart, 0);
30206 assertEquals(mCalibrateEnd, 0);
30207
30208 try {
30209 calibrator.calibrate();
30210 } catch (final CalibrationException e) {
30211 continue;
30212 }
30213
30214
30215 assertTrue(calibrator.isReady());
30216 assertFalse(calibrator.isRunning());
30217 assertEquals(mCalibrateStart, 1);
30218 assertEquals(mCalibrateEnd, 1);
30219
30220 final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
30221 final Matrix estimatedMa = calibrator.getEstimatedMa();
30222
30223
30224
30225
30226 if (!ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR)) {
30227 continue;
30228 }
30229 if (!ma.equals(estimatedMa, 20.0 * LARGE_ABSOLUTE_ERROR)) {
30230 continue;
30231 }
30232
30233 assertTrue(ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR));
30234 assertTrue(ma.equals(estimatedMa, 20.0 * LARGE_ABSOLUTE_ERROR));
30235
30236 numValid++;
30237
30238 break;
30239 }
30240
30241 assertTrue(numValid > 0);
30242 }
30243
30244 @Ignore
30245 @Test
30246 public void testNorms() throws InvalidSourceAndDestinationFrameTypeException {
30247
30248 final Random random = new Random();
30249 final UniformRandomizer randomizer = new UniformRandomizer(random);
30250 final double latitude = Math.toRadians(
30251 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
30252 final double longitude = Math.toRadians(
30253 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30254 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30255 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30256
30257 final double roll = Math.toRadians(
30258 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
30259 final double pitch = Math.toRadians(
30260 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
30261 final double yaw = Math.toRadians(
30262 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
30263 final CoordinateTransformation nedC = new CoordinateTransformation(
30264 roll, pitch, yaw, FrameType.BODY_FRAME,
30265 FrameType.LOCAL_NAVIGATION_FRAME);
30266
30267 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
30268 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
30269 .convertNEDtoECEFAndReturnNew(nedFrame);
30270
30271 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
30272 .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
30273 ecefFrame);
30274
30275 final double fNorm = trueKinematics.getSpecificForceNorm();
30276 final double wNorm = trueKinematics.getAngularRateNorm();
30277
30278 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(ecefFrame);
30279 final double g = gravity.getNorm();
30280
30281 assertEquals(g, fNorm, ABSOLUTE_ERROR);
30282 assertEquals(Constants.EARTH_ROTATION_RATE, wNorm, ABSOLUTE_ERROR);
30283 }
30284
30285 @Override
30286 public void onCalibrateStart(KnownPositionAccelerometerCalibrator calibrator) {
30287 checkLocked(calibrator);
30288 mCalibrateStart++;
30289 }
30290
30291 @Override
30292 public void onCalibrateEnd(KnownPositionAccelerometerCalibrator calibrator) {
30293 checkLocked(calibrator);
30294 mCalibrateEnd++;
30295 }
30296
30297 private void reset() {
30298 mCalibrateStart = 0;
30299 mCalibrateEnd = 0;
30300 }
30301
30302 private void checkLocked(final KnownPositionAccelerometerCalibrator calibrator) {
30303 assertTrue(calibrator.isRunning());
30304 try {
30305 calibrator.setInitialBiasX(0.0);
30306 fail("LockedException expected but not thrown");
30307 } catch (final LockedException ignore) {
30308 }
30309 try {
30310 calibrator.setInitialBiasY(0.0);
30311 fail("LockedException expected but not thrown");
30312 } catch (final LockedException ignore) {
30313 }
30314 try {
30315 calibrator.setInitialBiasZ(0.0);
30316 fail("LockedException expected but not thrown");
30317 } catch (final LockedException ignore) {
30318 }
30319 try {
30320 calibrator.setInitialBiasX(null);
30321 fail("LockedException expected but not thrown");
30322 } catch (final LockedException ignore) {
30323 }
30324 try {
30325 calibrator.setInitialBiasY(null);
30326 fail("LockedException expected but not thrown");
30327 } catch (final LockedException ignore) {
30328 }
30329 try {
30330 calibrator.setInitialBiasZ(null);
30331 fail("LockedException expected but not thrown");
30332 } catch (final LockedException ignore) {
30333 }
30334 try {
30335 calibrator.setInitialBias(0.0, 0.0, 0.0);
30336 fail("LockedException expected but not thrown");
30337 } catch (final LockedException ignore) {
30338 }
30339 try {
30340 calibrator.setInitialBias(null, null, null);
30341 fail("LockedException expected but not thrown");
30342 } catch (final LockedException ignore) {
30343 }
30344 try {
30345 calibrator.setInitialSx(0.0);
30346 fail("LockedException expected but not thrown");
30347 } catch (final LockedException ignore) {
30348 }
30349 try {
30350 calibrator.setInitialSy(0.0);
30351 fail("LockedException expected but not thrown");
30352 } catch (final LockedException ignore) {
30353 }
30354 try {
30355 calibrator.setInitialSz(0.0);
30356 fail("LockedException expected but not thrown");
30357 } catch (final LockedException ignore) {
30358 }
30359 try {
30360 calibrator.setInitialMxy(0.0);
30361 fail("LockedException expected but not thrown");
30362 } catch (final LockedException ignore) {
30363 }
30364 try {
30365 calibrator.setInitialMxz(0.0);
30366 fail("LockedException expected but not thrown");
30367 } catch (final LockedException ignore) {
30368 }
30369 try {
30370 calibrator.setInitialMyx(0.0);
30371 fail("LockedException expected but not thrown");
30372 } catch (final LockedException ignore) {
30373 }
30374 try {
30375 calibrator.setInitialMyz(0.0);
30376 fail("LockedException expected but not thrown");
30377 } catch (final LockedException ignore) {
30378 }
30379 try {
30380 calibrator.setInitialMzx(0.0);
30381 fail("LockedException expected but not thrown");
30382 } catch (final LockedException ignore) {
30383 }
30384 try {
30385 calibrator.setInitialMzy(0.0);
30386 fail("LockedException expected but not thrown");
30387 } catch (final LockedException ignore) {
30388 }
30389 try {
30390 calibrator.setInitialScalingFactors(0.0, 0.0, 0.0);
30391 fail("LockedException expected but not thrown");
30392 } catch (final LockedException ignore) {
30393 }
30394 try {
30395 calibrator.setInitialCrossCouplingErrors(
30396 0.0, 0.0, 0.0,
30397 0.0, 0.0, 0.0);
30398 fail("LockedException expected but not thrown");
30399 } catch (final LockedException ignore) {
30400 }
30401 try {
30402 calibrator.setInitialScalingFactorsAndCrossCouplingErrors(
30403 0.0, 0.0, 0.0,
30404 0.0, 0.0, 0.0,
30405 0.0, 0.0, 0.0);
30406 fail("LockedException expected but not thrown");
30407 } catch (final LockedException ignore) {
30408 }
30409 try {
30410 calibrator.setInitialBias((double[]) null);
30411 fail("LockedException expected but not thrown");
30412 } catch (final LockedException ignore) {
30413 }
30414 try {
30415 calibrator.setInitialBias((Matrix) null);
30416 fail("LockedException expected but not thrown");
30417 } catch (final LockedException ignore) {
30418 }
30419 try {
30420 calibrator.setInitialMa(null);
30421 fail("LockedException expected but not thrown");
30422 } catch (final LockedException ignore) {
30423 }
30424 try {
30425 calibrator.setPosition((ECEFPosition) null);
30426 fail("LockedException expected but not thrown");
30427 } catch (final LockedException ignore) {
30428 }
30429 try {
30430 calibrator.setPosition((NEDPosition) null);
30431 fail("LockedException expected but not thrown");
30432 } catch (final LockedException ignore) {
30433 }
30434 try {
30435 calibrator.setMeasurements(null);
30436 fail("LockedException expected but not thrown");
30437 } catch (final LockedException ignore) {
30438 }
30439 try {
30440 calibrator.setCommonAxisUsed(true);
30441 fail("LockedException expected but not thrown");
30442 } catch (final LockedException ignore) {
30443 }
30444 try {
30445 calibrator.setListener(this);
30446 fail("LockedException expected but not thrown");
30447 } catch (final LockedException ignore) {
30448 }
30449 try {
30450 calibrator.calibrate();
30451 fail("LockedException expected but not thrown");
30452 } catch (final LockedException ignore) {
30453 } catch (final Exception e) {
30454 fail("LockedException expected but not thrown");
30455 }
30456 }
30457
30458 private Matrix generateBa() {
30459 return Matrix.newFromArray(new double[]{
30460 900 * MICRO_G_TO_METERS_PER_SECOND_SQUARED,
30461 -1300 * MICRO_G_TO_METERS_PER_SECOND_SQUARED,
30462 800 * MICRO_G_TO_METERS_PER_SECOND_SQUARED});
30463 }
30464
30465 private Matrix generateBg() {
30466 return Matrix.newFromArray(new double[]{
30467 -9 * DEG_TO_RAD / 3600.0,
30468 13 * DEG_TO_RAD / 3600.0,
30469 -8 * DEG_TO_RAD / 3600.0});
30470 }
30471
30472 private Matrix generateMaGeneral() throws WrongSizeException {
30473 final Matrix result = new Matrix(3, 3);
30474 result.fromArray(new double[]{
30475 500e-6, -300e-6, 200e-6,
30476 -150e-6, -600e-6, 250e-6,
30477 -250e-6, 100e-6, 450e-6
30478 }, false);
30479
30480 return result;
30481 }
30482
30483 private Matrix generateMaCommonAxis() throws WrongSizeException {
30484 final Matrix result = new Matrix(3, 3);
30485 result.fromArray(new double[]{
30486 500e-6, -300e-6, 200e-6,
30487 0.0, -600e-6, 250e-6,
30488 0.0, 0.0, 450e-6
30489 }, false);
30490
30491 return result;
30492 }
30493
30494 private Matrix generateMg() throws WrongSizeException {
30495 final Matrix result = new Matrix(3, 3);
30496 result.fromArray(new double[]{
30497 400e-6, -300e-6, 250e-6,
30498 0.0, -300e-6, -150e-6,
30499 0.0, 0.0, -350e-6
30500 }, false);
30501
30502 return result;
30503 }
30504
30505 private Matrix generateGg() throws WrongSizeException {
30506 final Matrix result = new Matrix(3, 3);
30507 final double tmp = DEG_TO_RAD / (3600 * 9.80665);
30508 result.fromArray(new double[]{
30509 0.9 * tmp, -1.1 * tmp, -0.6 * tmp,
30510 -0.5 * tmp, 1.9 * tmp, -1.6 * tmp,
30511 0.3 * tmp, 1.1 * tmp, -1.3 * tmp
30512 }, false);
30513
30514 return result;
30515 }
30516
30517 private double getAccelNoiseRootPSD() {
30518 return 100.0 * MICRO_G_TO_METERS_PER_SECOND_SQUARED;
30519 }
30520
30521 private double getGyroNoiseRootPSD() {
30522 return 0.01 * DEG_TO_RAD / 60.0;
30523 }
30524 }